import sys
import os
import serial
import serial.tools.list_ports
import json
import math
from PyQt6.QtWidgets import (QApplication, QMainWindow, QWidget, QVBoxLayout, QHBoxLayout,
                             QPushButton, QComboBox, QLabel, QTextEdit, QGridLayout, QGroupBox,
                             QSlider, QLineEdit, QDoubleSpinBox, QTabWidget, QCheckBox, QStackedWidget)
from PyQt6.QtCore import QThread, pyqtSignal, QObject, pyqtSlot, Qt, QUrl, QTimer
from PyQt6.QtGui import QFont, QColor, QPalette, QIcon
from PyQt6.QtWebEngineWidgets import QWebEngineView
from PyQt6.QtWebEngineCore import QWebEnginePage
from PyQt6.QtWebChannel import QWebChannel
import struct
import pyqtgraph as pg
import csv
from datetime import datetime, timezone, timedelta
import time
import requests
import subprocess
import socket  # 添加socket模块用于TCP通信
import paho.mqtt.client as mqtt
import base64
import hmac
from urllib.parse import quote

# ==============================================================================
# Leaflet.js 地图模板
# ==============================================================================
MAP_HTML = """
<!DOCTYPE html>
<html>
<head>
    <title>GPS Map</title>
    <meta charset="utf-8" />
    <meta name="viewport" content="width=device-width, initial-scale=1.0">
    <link rel="stylesheet" href="https://unpkg.com/leaflet@1.7.1/dist/leaflet.css"/>
    <script src="https://unpkg.com/leaflet@1.7.1/dist/leaflet.js"></script>
    <script src="https://unpkg.com/leaflet-rotatedmarker@0.2.0/leaflet.rotatedMarker.js"></script>
    <script src="qrc:///qtwebchannel/qwebchannel.js"></script>
    <style>
        html, body, #map {
            height: 100%;
            width: 100%;
            margin: 0;
            padding: 0;
            background-color: #2b2b2b; /* Match dark theme */
        }
    </style>
</head>
<body>
    <div id="map"></div>
    <script>
        var map;
        var carMarker;
        var trajectoryLine;
        var startMarker;
        var destinationMarker;
        var tileLayers = {};

        function initMap() {
            console.log("Initializing Leaflet map...");
            // 定义瓦片图层
            tileLayers['高德街道'] = L.tileLayer('https://webrd01.is.autonavi.com/appmaptile?lang=zh_cn&size=1&scale=1&style=8&x={x}&y={y}&z={z}', {
                attribution: '© 高德地图'
            });
            tileLayers['高德卫星'] = L.tileLayer('https://webst01.is.autonavi.com/appmaptile?style=6&x={x}&y={y}&z={z}', {
                attribution: '© 高德地图'
            });
            tileLayers['OpenStreetMap'] = L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
                attribution: '&copy; <a href="https://www.openstreetmap.org/copyright">OpenStreetMap</a> contributors'
            });

            map = L.map('map', {
                center: [31.2304, 121.4737], // 默认上海
                zoom: 12,
                layers: [tileLayers['高德街道']] // 默认图层
            });
            console.log("Map initialized.");

            // 设置与 Python 的通信
            new QWebChannel(qt.webChannelTransport, function (channel) {
                window.qt_api = channel.objects.qt_api;
                console.log("WebChannel connected to Python.");
                
                // 地图点击事件，发送坐标到 Python
                map.on('click', function(e) {
                    if (window.qt_api) {
                        window.qt_api.onMapClicked(e.latlng.lat, e.latlng.lng);
                    }
                });
            });
        }

        function changeTileLayer(name) {
            if (map && tileLayers[name]) {
                map.eachLayer(function(layer){
                    if(layer instanceof L.TileLayer){
                        map.removeLayer(layer);
                    }
                });
                map.addLayer(tileLayers[name]);
            }
        }
        
        function setDestinationMarker(lat, lon) {
            var newPoint = [lat, lon];
            if (destinationMarker) {
                destinationMarker.setLatLng(newPoint);
            } else {
                destinationMarker = L.marker(newPoint, {
                    icon: L.icon({
                        iconUrl: 'https://raw.githubusercontent.com/pointhi/leaflet-color-markers/master/img/marker-icon-2x-red.png',
                        iconSize: [25, 41], iconAnchor: [12, 41], popupAnchor: [1, -34], shadowSize: [41, 41]
                    })
                }).addTo(map);
                destinationMarker.bindPopup('<b>目标点</b>');
            }
        }

        function updateTrajectory(lat, lon, heading, gps_status) {
            var newPoint = [lat, lon];
            var iconUrl = ''; // Blue, normal
            if (gps_status === 'weak') {
               iconUrl = ''; // Yellow, weak signal
            }

            if (!carMarker) {
                // 第一个点: 创建所有元素
                startMarker = L.marker(newPoint, {
                    icon: L.icon({
                        iconUrl: 'https://raw.githubusercontent.com/pointhi/leaflet-color-markers/master/img/marker-icon-2x-green.png',
                        iconSize: [25, 41], iconAnchor: [12, 41], popupAnchor: [1, -34], shadowSize: [41, 41]
                    })
                }).addTo(map);
                startMarker.bindPopup('<b>起点</b>');

                trajectoryLine = L.polyline([newPoint], {color: 'blue', weight: 3}).addTo(map);
                
                carMarker = L.marker(newPoint, {
                    rotationAngle: heading,
                    rotationOrigin: 'center center',
                    icon: L.icon({
                         iconUrl: iconUrl,
                         iconSize: [32, 32],
                         iconAnchor: [16, 16]
                    })
                }).addTo(map);
                carMarker.bindPopup('<b>当前位置</b>');
                map.setView(newPoint, 18);
            } else {
                // 后续点: 更新现有元素
                carMarker.setLatLng(newPoint);
                carMarker.setIcon(L.icon({
                    iconUrl: iconUrl,
                    iconSize: [32, 32],
                    iconAnchor: [16, 16]
                }));

                if (heading !== null) {
                    carMarker.setRotationAngle(heading);
                }
                trajectoryLine.addLatLng(newPoint);
            }
        }
        
        function centerMap(lat, lon) {
            if (map) {
                map.panTo([lat, lon]);
            }
        }

        function clearTrajectory() {
            if (map) {
                if (carMarker) map.removeLayer(carMarker);
                if (startMarker) map.removeLayer(startMarker);
                if (trajectoryLine) map.removeLayer(trajectoryLine);
                if (destinationMarker) map.removeLayer(destinationMarker);
                carMarker = null;
                startMarker = null;
                trajectoryLine = null;
                destinationMarker = null;
                console.log("Trajectory cleared.");
            }
        }
        
        document.addEventListener("DOMContentLoaded", function() {
             initMap();
        });

    </script>
</body>
</html>
"""

# Dark Theme Stylesheet
dark_stylesheet = """
    QWidget {
        background-color: #2b2b2b;
        color: #f0f0f0;
        font-family: 'Segoe UI', 'Microsoft YaHei', 'Arial';
        font-size: 10pt;
    }
    QMainWindow {
        background-color: #2b2b2b;
    }
    QGroupBox {
        border: 1px solid #444;
        border-radius: 5px;
        margin-top: 1ex;
        font-weight: bold;
    }
    QGroupBox::title {
        subcontrol-origin: margin;
        subcontrol-position: top center;
        padding: 0 3px;
    }
    QPushButton {
        background-color: #555;
        border: 1px solid #666;
        border-radius: 4px;
        padding: 5px;
        min-height: 15px;
    }
    QPushButton:hover {
        background-color: #666;
        border-color: #777;
    }
    QPushButton:pressed, QPushButton:checked {
        background-color: #007acc;
        border-color: #005c99;
    }
    QComboBox {
        border: 1px solid #444;
        border-radius: 3px;
        padding: 1px 18px 1px 3px;
        min-width: 6em;
    }
    QComboBox::drop-down {
        subcontrol-origin: padding;
        subcontrol-position: top right;
        width: 15px;
        border-left-width: 1px;
        border-left-color: #444;
        border-left-style: solid;
        border-top-right-radius: 3px;
        border-bottom-right-radius: 3px;
    }
    QLineEdit, QDoubleSpinBox, QTextEdit {
        background-color: #3c3c3c;
        border: 1px solid #444;
        border-radius: 4px;
        padding: 5px;
    }
    QTabWidget::pane {
        border-top: 1px solid #444;
    }
    QTabBar::tab {
        background: #3c3c3c;
        border: 1px solid #444;
        border-bottom-color: #2b2b2b; 
        border-top-left-radius: 4px;
        border-top-right-radius: 4px;
        min-width: 8ex;
        padding: 5px;
    }
    QTabBar::tab:selected {
        background: #007acc;
        color: white;
    }
    QTabBar::tab:hover {
        background: #555;
    }
    QSlider::groove:horizontal {
        border: 1px solid #444;
        height: 8px;
        background: #3c3c3c;
        margin: 2px 0;
        border-radius: 4px;
    }
    QSlider::handle:horizontal {
        background: #007acc;
        border: 1px solid #007acc;
        width: 18px;
        margin: -5px 0;
        border-radius: 9px;
    }
"""

# ==============================================================================
# 自定义 WebEnginePage 以捕获JS控制台日志
# ==============================================================================
class WebEnginePage(QWebEnginePage):
    """Custom WebEnginePage to handle javascript console messages."""
    def javaScriptConsoleMessage(self, level, message, lineNumber, sourceID):
        print(f"JS Console ({sourceID}:{lineNumber}): {message}")

# ==============================================================================
# 与地图JS交互的API
# ==============================================================================
class MapApi(QObject):
    map_clicked = pyqtSignal(float, float)

    @pyqtSlot(float, float)
    def onMapClicked(self, lat, lon):
        self.map_clicked.emit(lat, lon)

# ==============================================================================
# 串口通信工作线程 (无修改)
# ==============================================================================
class SerialWorker(QObject):
    data_received = pyqtSignal(bytes)
    log_message = pyqtSignal(str)

    def __init__(self, serial_instance):
        super().__init__()
        self.serial_instance = serial_instance
        self._is_running = True

    def run(self):
        self.log_message.emit("串口工作线程已启动。")
        while self._is_running:
            if self.serial_instance and self.serial_instance.is_open:
                try:
                    if self.serial_instance.in_waiting > 0:
                        data = self.serial_instance.read(self.serial_instance.in_waiting)
                        if data:
                            self.data_received.emit(data)
                except serial.SerialException as e:
                    self.log_message.emit(f"串口错误: {e}")
                    self.stop()
            else:
                self.stop()
        self.log_message.emit("串口工作线程已停止。")

    def stop(self):
        self._is_running = False

# ==============================================================================
# OneNet 通信工作线程 (已有)
# ==============================================================================
class OneNetWorker(QObject):
    data_received = pyqtSignal(dict)
    log_message = pyqtSignal(str)

    def __init__(self, api_key, device_id, interval=5):
        super().__init__()
        self.api_key = api_key
        self.device_id = device_id
        self.interval = interval
        self._is_running = True

    def run(self):
        self.log_message.emit("OneNet工作线程已启动。")
        while self._is_running:
            try:
                url = f"https://api.heclouds.com/devices/{self.device_id}/datastreams"
                headers = {"api-key": self.api_key}
                response = requests.get(url, headers=headers, timeout=5)
                
                if response.status_code == 200:
                    data = response.json()
                    if data.get("errno", 0) == 0 and "data" in data:
                        self.data_received.emit(data["data"])
                    else:
                        self.log_message.emit(f"OneNet API 错误: {data.get('error', '未知错误')}")
                else:
                    self.log_message.emit(f"OneNet 请求失败，状态码: {response.status_code}")
            
            except requests.exceptions.RequestException as e:
                self.log_message.emit(f"OneNet 网络请求错误: {e}")
            
            time.sleep(self.interval)
        self.log_message.emit("OneNet工作线程已停止。")

    def stop(self):
        self._is_running = False

# ==============================================================================
# OneNet MQTT 通信工作线程 (新增)
# ==============================================================================
class OneNetMQTTWorker(QObject):
    data_received = pyqtSignal(dict)
    log_message = pyqtSignal(str)

    def __init__(self, product_id, device_id, token):
        super().__init__()
        # MQTT连接参数
        self.host = "183.230.40.96"
        self.port = 1883
        self.product_id = product_id
        self.device_id = device_id
        self.token = token
        self._is_running = True
        self.client = None
        
        # 重连相关设置
        self.reconnect_delay = 5  # 重连延迟，单位秒
        self.reconnect_count = 0
        self.max_reconnect = 10   # 最大重连次数
        self.reconnect_timer = None
        
        # 主题定义
        self.topic_property_post = f"$sys/{product_id}/{device_id}/thing/property/post"
        self.topic_property_post_reply = f"$sys/{product_id}/{device_id}/thing/property/post/reply"

    def on_connect(self, client, userdata, flags, rc):
        if rc == 0:
            self.log_message.emit(f"MQTT连接成功")
            self.reconnect_count = 0  # 重置重连计数
            # 订阅回复主题
            client.subscribe(self.topic_property_post_reply)
            self.log_message.emit(f"已订阅主题: {self.topic_property_post_reply}")
            
            # 不再发送自定义心跳包
        else:
            self.log_message.emit(f"MQTT连接失败，返回码: {rc}")
            if self._is_running:
                self.schedule_reconnect()

    def on_message(self, client, userdata, msg):
        self.log_message.emit(f"收到MQTT消息，主题: {msg.topic}")
        try:
            payload = msg.payload.decode('utf-8')
            self.log_message.emit(f"消息内容: {payload}")
            
            # 尝试解析JSON数据
            if self.topic_property_post_reply in msg.topic:
                data = json.loads(payload)
                if 'data' in data:
                    self.data_received.emit(data['data'])
            else:
                # 处理其他主题的消息
                pass
        except Exception as e:
            self.log_message.emit(f"处理MQTT消息出错: {e}")

    def on_disconnect(self, client, userdata, rc):
        try:
            reason = mqtt.error_string(rc)
        except:
            reason = "Unknown"
        
        if rc != 0:
            self.log_message.emit(f"MQTT意外断开连接，返回码: {rc} ({reason})")
            if self._is_running:
                self.schedule_reconnect()
        else:
            self.log_message.emit("MQTT正常断开连接")

    def schedule_reconnect(self):
        """安排重新连接"""
        if not self._is_running:
            return
            
        if self.reconnect_count >= self.max_reconnect:
            self.log_message.emit(f"已达到最大重连次数 {self.max_reconnect}，停止重连")
            return
            
        self.reconnect_count += 1
        delay = min(self.reconnect_delay * self.reconnect_count, 60)  # 指数退避，最大60秒
        
        self.log_message.emit(f"计划在 {delay} 秒后进行第 {self.reconnect_count} 次重连...")
        
        # 使用QTimer进行延迟重连
        if self.reconnect_timer is None:
            self.reconnect_timer = QTimer()
            self.reconnect_timer.setSingleShot(True)
            self.reconnect_timer.timeout.connect(self.reconnect)
        
        if not self.reconnect_timer.isActive():
            self.reconnect_timer.start(delay * 1000)

    def reconnect(self):
        """执行重新连接"""
        if not self._is_running:
            return
            
        self.log_message.emit("正在尝试重新连接MQTT服务器...")
        try:
            if self.client:
                # 确保旧客户端资源被释放
                self.client.loop_stop()
                
            # 重新创建MQTT客户端
            self.setup_client()
            self.client.connect(self.host, self.port, keepalive=60)
            self.client.loop_start()
        except Exception as e:
            self.log_message.emit(f"重连失败: {e}")
            if self._is_running:
                self.schedule_reconnect()

    def setup_client(self):
        """设置MQTT客户端"""
        self.client = mqtt.Client(client_id=self.device_id, clean_session=True, protocol=mqtt.MQTTv311)
        
        # 设置用户名和密码，OneNET平台用户名为产品ID，密码为token
        self.client.username_pw_set(self.product_id, self.token)
        
        # 设置回调函数
        self.client.on_connect = self.on_connect
        self.client.on_message = self.on_message
        self.client.on_disconnect = self.on_disconnect
        
        # 设置保活参数
        self.client.reconnect_delay_set(min_delay=1, max_delay=60)

    def send_heartbeat(self):
        """不再使用该方法，保留以兼容已有代码"""
        pass

    def run(self):
        self.log_message.emit("OneNet MQTT工作线程已启动。")
        
        try:
            self.setup_client()
            
            # 连接MQTT服务器
            self.log_message.emit(f"正在连接到MQTT服务器 {self.host}:{self.port}...")
            self.client.connect(self.host, self.port, keepalive=60) # 使用标准的60秒keepalive
            
            # 启动网络循环
            self.client.loop_start()
            
            # 不再设置心跳定时器
            
            # 保持工作线程运行
            while self._is_running:
                time.sleep(0.1)
                
        except Exception as e:
            self.log_message.emit(f"MQTT连接出错: {e}")
            if self._is_running:
                self.schedule_reconnect()
        finally:
            if self.reconnect_timer and self.reconnect_timer.isActive():
                self.reconnect_timer.stop()
            if self.client:
                self.client.loop_stop()
                if self.client.is_connected():
                    self.client.disconnect()
            self.log_message.emit("OneNet MQTT工作线程已停止。")

    def stop(self):
        self._is_running = False
        if self.reconnect_timer and self.reconnect_timer.isActive():
            self.reconnect_timer.stop()
        if self.client:
            self.client.loop_stop()
            if self.client.is_connected():
                self.client.disconnect()

    def send_data(self, payload_str):
        """向属性设置主题发布数据"""
        if not self.client or not self.client.is_connected():
            self.log_message.emit("MQTT发送失败: 客户端未连接")
            return

        # 物模型属性设置主题
        topic = f"$sys/{self.product_id}/{self.device_id}/thing/property/set"
        
        try:
            # 尝试将输入解析为JSON。如果成功，则直接用它作为params。
            params = json.loads(payload_str)
        except json.JSONDecodeError:
            # 如果输入不是有效的JSON，则将其作为单个命令字符串处理
            params = {"command": payload_str}

        # 构建符合OneNet物模型协议的完整JSON payload
        msg_id = int(time.time())
        full_payload = {
            "id": str(msg_id),
            "version": "1.0",
            "params": params
        }
        
        payload_json = json.dumps(full_payload)

        try:
            self.client.publish(topic, payload_json, qos=0)
            self.log_message.emit(f"MQTT Published to {topic}: {payload_json}")
        except Exception as e:
            self.log_message.emit(f"MQTT发布消息失败: {e}")

# ==============================================================================
# Pulsar Java SDK 通信工作线程 (新增)
# ==============================================================================
class PulsarWorker(QObject):
    data_received = pyqtSignal(dict)
    log_message = pyqtSignal(str)

    def __init__(self, project_path):
        super().__init__()
        self.project_path = project_path
        self._is_running = True
        self.proc = None

    def run(self):
        self.log_message.emit(f"Pulsar工作线程已启动，项目路径: {self.project_path}")
        
        jar_name = "iot-pulsar-sdk-java-3.0.1-1.0.jar"
        jar_path = os.path.join(self.project_path, 'target', jar_name)
        
        if not os.path.exists(jar_path):
            self.log_message.emit(f"错误: 找不到JAR文件 at {jar_path}. 请先使用 'mvn package' 命令编译项目。")
            self.stop()
            return

        command = ['java', '-jar', jar_path]
        
        try:
            # 使用 Popen 启动子进程
            self.proc = subprocess.Popen(
                command,
                cwd=self.project_path,
                stdout=subprocess.PIPE,
                stderr=subprocess.STDOUT, # 将错误流重定向到标准输出
                text=True,
                encoding='utf-8',
                errors='replace' # 替换无法解码的字符
            )
            self.log_message.emit(f"已启动Java进程: {' '.join(command)}")

            # 实时读取输出
            while self._is_running:
                if self.proc.stdout:
                    line = self.proc.stdout.readline()
                    if not line: # 如果没有输出，说明进程可能已结束
                        break
                    
                    self.log_message.emit(f"PULSAR_RAW: {line.strip()}") # 记录原始输出
                    
                    # 查找包含原始数据的行
                    if "IOT originalMsg" in line:
                        try:
                            # 提取 JSON 部分
                            json_str = line[line.find('{'):]
                            data = json.loads(json_str)
                            self.data_received.emit(data)
                        except json.JSONDecodeError:
                            self.log_message.emit(f"JSON 解析错误: {line.strip()}")
                        except Exception as e:
                            self.log_message.emit(f"处理Pulsar消息时出错: {e}")
                else:
                    break # stdout关闭，退出循环

        except FileNotFoundError:
            self.log_message.emit("错误: 'java' 命令未找到。请确保Java已安装并配置在系统PATH中。")
        except Exception as e:
            self.log_message.emit(f"启动Pulsar Java进程时出错: {e}")
        
        self.log_message.emit("Pulsar工作线程已停止。")
        self.stop() # 确保进程被终止

    def stop(self):
        if self._is_running:
            self._is_running = False
            if self.proc and self.proc.poll() is None: # 检查进程是否仍在运行
                self.log_message.emit("正在终止Pulsar Java进程...")
                self.proc.terminate()
                try:
                    self.proc.wait(timeout=5) # 等待5秒
                except subprocess.TimeoutExpired:
                    self.log_message.emit("终止超时，强制结束进程。")
                    self.proc.kill()
            self.proc = None

# ==============================================================================
# OneNet HTTP Push 通信工作线程 (新增)
# ==============================================================================
class OneNetHttpPushWorker(QObject):
    data_received = pyqtSignal(dict)
    log_message = pyqtSignal(str)

    def __init__(self, project_path, token, aes_key, port):
        super().__init__()
        self.project_path = project_path
        self.token = token
        self.aes_key = aes_key
        self.port = port
        self._is_running = True
        self.proc = None

    def run(self):
        self.log_message.emit(f"OneNet HTTP Push工作线程已启动，项目路径: {self.project_path}")
        
        jar_name = "datapush-receive-demo-1.0.0.jar"
        jar_path = os.path.join(self.project_path, 'target', jar_name)
        
        if not os.path.exists(jar_path):
            self.log_message.emit(f"错误: 找不到JAR文件 at {jar_path}. 请先使用 'mvn package' 命令编译项目。")
            return

        command = [
            'java',
            '-jar',
            jar_path,
            f'--server.port={self.port}',
            f'--onenet.token={self.token}',
            f'--onenet.aesKey={self.aes_key}'
        ]
        
        try:
            self.proc = subprocess.Popen(
                command,
                cwd=self.project_path,
                stdout=subprocess.PIPE,
                stderr=subprocess.STDOUT,
                text=True,
                encoding='utf-8',
                errors='replace'
            )
            self.log_message.emit(f"已启动Java进程: {' '.join(command)}")

            while self._is_running:
                if self.proc.stdout:
                    line = self.proc.stdout.readline()
                    if not line:
                        break
                    
                    line = line.strip()
                    self.log_message.emit(f"HTTP_PUSH_RAW: {line}") # 记录原始输出
                    
                    prefix = "ONENET_PUSH_DATA: "
                    if line.startswith(prefix):
                        try:
                            json_str = line[len(prefix):]
                            data = json.loads(json_str)
                            self.data_received.emit(data)
                        except json.JSONDecodeError:
                            self.log_message.emit(f"JSON a解析错误: {line}")
                        except Exception as e:
                            self.log_message.emit(f"处理HTTP Push消息时出错: {e}")
                else:
                    break

        except FileNotFoundError:
            self.log_message.emit("错误: 'java' 命令未找到。请确保Java已安装并配置在系统PATH中。")
        except Exception as e:
            self.log_message.emit(f"启动 OneNet Push Java进程时出错: {e}")
        
        self.log_message.emit("OneNet HTTP Push工作线程已停止。")
        self.stop()

    def stop(self):
        if self._is_running:
            self._is_running = False
            if self.proc and self.proc.poll() is None:
                self.log_message.emit("正在终止 OneNet Push Java进程...")
                self.proc.terminate()
                try:
                    self.proc.wait(timeout=5)
                except subprocess.TimeoutExpired:
                    self.log_message.emit("终止超时，强制结束进程。")
                    self.proc.kill()
            self.proc = None

# ==============================================================================
# MQTT Relay 通信工作线程 (新增)
# ==============================================================================
class MqttRelayWorker(QObject):
    data_received = pyqtSignal(dict)
    log_message = pyqtSignal(str)

    def __init__(self, host, port, username, password, topic):
        super().__init__()
        self.host = host
        self.port = int(port)
        self.username = username
        self.password = password
        self.topic = topic
        self._is_running = True
        self.client = None
        self.reconnect_timer = None

    def on_connect(self, client, userdata, flags, rc):
        if rc == 0:
            self.log_message.emit(f"MQTT Relay 连接成功到 {self.host}:{self.port}")
            client.subscribe(self.topic)
            self.log_message.emit(f"已订阅主题: {self.topic}")
        else:
            self.log_message.emit(f"MQTT Relay 连接失败, 返回码: {rc}")
            if self._is_running:
                self.schedule_reconnect()

    def on_message(self, client, userdata, msg):
        self.log_message.emit(f"收到 MQTT Relay 消息, 主题: {msg.topic}")
        try:
            payload = msg.payload.decode('utf-8')
            self.log_message.emit(f"消息内容: {payload}")
            data = json.loads(payload)
            # 假设Java程序已经将数据处理成我们需要的格式
            # OneNet的payload通常在 'payload' 键下
            if 'payload' in data:
                 self.data_received.emit(data['payload'])
            else:
                 self.log_message.emit(f"未找到 'payload' 字段: {payload}")

        except json.JSONDecodeError:
            self.log_message.emit(f"JSON 解析错误: {msg.payload.decode('utf-8')}")
        except Exception as e:
            self.log_message.emit(f"处理 MQTT Relay 消息出错: {e}")

    def on_disconnect(self, client, userdata, rc):
        if rc != 0:
            self.log_message.emit(f"MQTT Relay 意外断开连接, 返回码: {rc}")
            if self._is_running:
                self.schedule_reconnect()
        else:
            self.log_message.emit("MQTT Relay 正常断开")

    def schedule_reconnect(self):
        if not self._is_running: return
        self.log_message.emit("计划 5 秒后重连 MQTT Relay...")
        if self.reconnect_timer is None:
            self.reconnect_timer = QTimer()
            self.reconnect_timer.setSingleShot(True)
            self.reconnect_timer.timeout.connect(self.run) # 重新执行run来连接
        if not self.reconnect_timer.isActive():
            self.reconnect_timer.start(5000)

    def run(self):
        self.log_message.emit("MQTT Relay 工作线程启动...")
        try:
            self.client = mqtt.Client()
            self.client.on_connect = self.on_connect
            self.client.on_message = self.on_message
            self.client.on_disconnect = self.on_disconnect
            
            if self.username and self.password:
                self.client.username_pw_set(self.username, self.password)

            self.client.connect(self.host, self.port, 60)
            self.client.loop_forever()
        except Exception as e:
            self.log_message.emit(f"MQTT Relay 连接出错: {e}")
            if self._is_running:
                self.schedule_reconnect()

    def stop(self):
        self._is_running = False
        if self.reconnect_timer and self.reconnect_timer.isActive():
            self.reconnect_timer.stop()
        if self.client:
            self.client.loop_stop()
            self.client.disconnect()


# ==============================================================================
# 主窗口
# ==============================================================================
class MainWindow(QMainWindow):
    def __init__(self):
        super().__init__()
        self.setWindowTitle("GPS 智能小车上位机 v1.4 - API轮询版") # Version bump
        self.setGeometry(100, 100, 1800, 1000)

        # 通信相关
        self.connection_mode = "串口" # 默认模式
        self.serial_port = serial.Serial()
        self.comm_thread = None
        self.comm_worker = None
        self.api_cmd_config = {'user_id': '', 'access_key': '', 'product_id': '', 'device_name': ''}
        self.api_poll_timer = QTimer(self)
        self.api_poll_timer.setInterval(1000) # 轮询间隔1秒
        self.api_poll_timer.timeout.connect(self.poll_device_properties_via_api)
        
        self.rx_buffer = bytearray()
        
        # 数据存储
        self.time_counter = 0
        self.time_points = []
        self.plot_data = {'pitch': [], 'roll': [], 'yaw': [], 'target_yaw': [], 'yaw_error': []}
        self.latest_data = {
            'lat': 0.0, 'lon': 0.0, 'gps_speed': 0.0, 'satellites': 0,
            'pitch': 0.0, 'roll': 0.0, 'yaw': 0.0, 'heading': 0.0,
            'motorL': 0, 'motorR': 0, 'battery': 0.0
        }
        self.system_status = {'mpu': 0, 'hmc': 0, 'gps': 0}
        self.target_yaw = 0.0

        # 地图与轨迹
        self.map_view = QWebEngineView()
        self.map_ready = False
        self.trajectory = []
        self.destination = None # (lat, lon)
        
        # 导航
        self.nav_timer = QTimer(self)
        self.nav_timer.setInterval(1000) # 导航指令更新频率 1Hz
        self.nav_timer.timeout.connect(self.navigate_to_destination)

        # 日志记录
        self.is_logging = False
        self.log_file = None
        self.csv_writer = None

        self.initUI()
        self.update_ports()
        self.init_map()
        self.update_connection_ui()

    def initUI(self):
        self.setStyleSheet(dark_stylesheet)
        central_widget = QWidget()
        self.setCentralWidget(central_widget)
        main_layout = QHBoxLayout(central_widget)

        # =====================================================================
        # 左侧面板 (控制与状态)
        # =====================================================================
        left_panel_layout = QVBoxLayout()
        left_panel_widget = QWidget()
        left_panel_widget.setLayout(left_panel_layout)
        left_panel_widget.setMaximumWidth(450)
        main_layout.addWidget(left_panel_widget)

        # -- 1. 连接设置 (始终在顶部) --
        conn_group = QGroupBox("连接设置")
        conn_layout = QGridLayout(conn_group)
        conn_group.setLayout(conn_layout)

        self.mode_combo = QComboBox()
        self.mode_combo.addItems(["串口", "自定义TCP服务器", "OneNet MQTT", "OneNet API (指令)", "Pulsar 数据流", "OneNet HTTP 推送", "云服务中继 (MQTT)"])
        
        # --- 创建 QStackedWidget 来管理不同模式的UI ---
        self.connection_stack = QStackedWidget()
        self.mode_combo.currentIndexChanged.connect(self.connection_stack.setCurrentIndex)
        self.mode_combo.currentTextChanged.connect(self.update_connection_ui)
        
        # --- 模式 0: 串口 ---
        self.serial_widget = QWidget()
        serial_layout = QGridLayout(self.serial_widget)
        serial_layout.setContentsMargins(0,0,0,0)
        self.ports_combo = QComboBox()
        self.refresh_button = QPushButton("刷新")
        self.refresh_button.clicked.connect(self.update_ports)
        serial_layout.addWidget(QLabel("串口:"), 0, 0)
        serial_layout.addWidget(self.ports_combo, 0, 1)
        serial_layout.addWidget(self.refresh_button, 0, 2)

        # 添加串口参数设置
        serial_layout.addWidget(QLabel("波特率:"), 1, 0)
        self.baudrate_combo = QComboBox()
        self.baudrate_combo.addItems(['115200', '9600', '19200', '38400', '57600', '230400', '460800', '921600'])
        self.baudrate_combo.setCurrentText('115200')
        serial_layout.addWidget(self.baudrate_combo, 1, 1, 1, 2)

        serial_layout.addWidget(QLabel("数据位:"), 2, 0)
        self.databits_combo = QComboBox()
        self.databits_combo.addItems(['8', '7', '6', '5'])
        self.databits_combo.setCurrentText('8')
        serial_layout.addWidget(self.databits_combo, 2, 1)
        
        serial_layout.addWidget(QLabel("校验位:"), 2, 2)
        self.parity_combo = QComboBox()
        self.parity_combo.addItems(['None', 'Even', 'Odd', 'Mark', 'Space'])
        self.parity_combo.setCurrentText('None')
        serial_layout.addWidget(self.parity_combo, 2, 3)
        
        serial_layout.addWidget(QLabel("停止位:"), 3, 0)
        self.stopbits_combo = QComboBox()
        self.stopbits_combo.addItems(['1', '1.5', '2'])
        self.stopbits_combo.setCurrentText('1')
        serial_layout.addWidget(self.stopbits_combo, 3, 1)
        self.connection_stack.addWidget(self.serial_widget)
        
        # --- 模式 1: 自定义TCP服务器 ---
        self.tcp_widget = QWidget()
        tcp_layout = QGridLayout(self.tcp_widget)
        tcp_layout.setContentsMargins(0,0,0,0)
        
        self.tcp_server_input = QLineEdit("101.34.228.243")
        self.tcp_port_input = QLineEdit("8889")
        self.tcp_device_id_input = QLineEdit("car001")
        
        tcp_layout.addWidget(QLabel("服务器地址:"), 0, 0)
        tcp_layout.addWidget(self.tcp_server_input, 0, 1)
        tcp_layout.addWidget(QLabel("端口:"), 1, 0)
        tcp_layout.addWidget(self.tcp_port_input, 1, 1)
        tcp_layout.addWidget(QLabel("设备ID:"), 2, 0)
        tcp_layout.addWidget(self.tcp_device_id_input, 2, 1)
        self.connection_stack.addWidget(self.tcp_widget)

        # --- 模式 2: OneNet MQTT ---
        self.onenet_mqtt_widget = QWidget()
        onenet_mqtt_layout = QGridLayout(self.onenet_mqtt_widget)
        onenet_mqtt_layout.setContentsMargins(0,0,0,0)
        
        self.mqtt_product_id_input = QLineEdit("F3i1fxm3zV")
        self.mqtt_device_id_input = QLineEdit("861556078753042")
        self.mqtt_token_input = QLineEdit("version=2018-10-31&res=products%2FF3i1fxm3zV%2Fdevices%2F861556078753042&et=2068172679&method=sha256&sign=VGZzMm%2FH54FM%2F3FTlDFgDxOryZqH4QwluKEHMo4VSkk%3D")
        self.mqtt_token_input.setEchoMode(QLineEdit.EchoMode.Password)
        
        token_view_button = QPushButton("👁️")
        token_view_button.setFixedWidth(30)
        token_view_button.setCheckable(True)
        token_view_button.clicked.connect(lambda checked: self.mqtt_token_input.setEchoMode(
            QLineEdit.EchoMode.Normal if checked else QLineEdit.EchoMode.Password
        ))
        
        onenet_mqtt_layout.addWidget(QLabel("产品ID:"), 0, 0)
        onenet_mqtt_layout.addWidget(self.mqtt_product_id_input, 0, 1)
        onenet_mqtt_layout.addWidget(QLabel("设备ID:"), 1, 0)
        onenet_mqtt_layout.addWidget(self.mqtt_device_id_input, 1, 1)
        onenet_mqtt_layout.addWidget(QLabel("Token:"), 2, 0)
        onenet_mqtt_layout.addWidget(self.mqtt_token_input, 2, 1)
        onenet_mqtt_layout.addWidget(token_view_button, 2, 2)
        self.connection_stack.addWidget(self.onenet_mqtt_widget)

        # --- 模式 3: OneNet API 指令发送 ---
        self.onenet_api_cmd_widget = QWidget()
        api_cmd_layout = QGridLayout(self.onenet_api_cmd_widget)
        api_cmd_layout.setContentsMargins(0, 0, 0, 0)
        self.api_cmd_user_id_input = QLineEdit("442584")
        self.api_cmd_user_id_input.setPlaceholderText("请输入您OneNet账户的用户ID (User ID)")
        self.api_cmd_access_key_input = QLineEdit("cGsQVGLUJsT/QDDOw/JmlhFRLnQ/ZjZ5tCItpJ8oYajbLUwHmwAx2J9E3uocQxfE")
        self.api_cmd_access_key_input.setPlaceholderText("请输入您的主账户或项目/产品的Access Key")
        self.api_cmd_access_key_input.setEchoMode(QLineEdit.EchoMode.Password)
        self.api_cmd_product_id_input = QLineEdit("F3i1fxm3zV")
        self.api_cmd_device_name_input = QLineEdit("861556078753042")
        
        api_cmd_layout.addWidget(QLabel("User ID:"), 0, 0)
        api_cmd_layout.addWidget(self.api_cmd_user_id_input, 0, 1, 1, 2)
        api_cmd_layout.addWidget(QLabel("Access Key:"), 1, 0)
        api_cmd_layout.addWidget(self.api_cmd_access_key_input, 1, 1, 1, 2)
        api_cmd_layout.addWidget(QLabel("产品ID:"), 2, 0)
        api_cmd_layout.addWidget(self.api_cmd_product_id_input, 2, 1, 1, 2)
        api_cmd_layout.addWidget(QLabel("设备名称:"), 3, 0)
        api_cmd_layout.addWidget(self.api_cmd_device_name_input, 3, 1, 1, 2)
        self.connection_stack.addWidget(self.onenet_api_cmd_widget)
        
        # --- 模式 4: Pulsar 数据流 ---
        self.pulsar_widget = QWidget()
        pulsar_layout = QGridLayout(self.pulsar_widget)
        pulsar_layout.setContentsMargins(0,0,0,0)
        self.pulsar_project_path_input = QLineEdit()
        self.pulsar_project_path_input.setPlaceholderText("请指定 iot-pulsar-sdk-java-3.0.1 文件夹路径")
        
        # Helper to find path when running from source or PyInstaller bundle
        if getattr(sys, 'frozen', False) and hasattr(sys, '_MEIPASS'):
            # Running in a PyInstaller bundle
            bundle_dir = sys._MEIPASS
            default_pulsar_path = os.path.join(bundle_dir, 'iot-pulsar-sdk-java-3.0.1')
        else:
            # Running in a normal Python environment
            default_pulsar_path = os.path.abspath(os.path.join(os.path.dirname(__file__), 'iot-pulsar-sdk-java-3.0.1'))

        self.pulsar_project_path_input.setText(default_pulsar_path)

        pulsar_layout.addWidget(QLabel("项目路径:"), 0, 0)
        pulsar_layout.addWidget(self.pulsar_project_path_input, 0, 1)
        self.connection_stack.addWidget(self.pulsar_widget)

        # --- 模式 5: OneNet HTTP Push ---
        self.onenet_push_widget = QWidget()
        onenet_push_layout = QGridLayout(self.onenet_push_widget)
        onenet_push_layout.setContentsMargins(0,0,0,0)
        
        default_push_path = os.path.abspath(os.path.join(os.path.dirname(__file__), 'datapush-receive-demo'))
        self.push_project_path_input = QLineEdit(default_push_path)
        self.push_token_input = QLineEdit("test") # 默认值
        self.push_aes_key_input = QLineEdit() # 默认留空为明文
        self.push_aes_key_input.setPlaceholderText("留空则为明文模式")
        self.push_port_input = QLineEdit("8080")
        
        onenet_push_layout.addWidget(QLabel("项目路径:"), 0, 0)
        onenet_push_layout.addWidget(self.push_project_path_input, 0, 1)
        onenet_push_layout.addWidget(QLabel("本地端口:"), 1, 0)
        onenet_push_layout.addWidget(self.push_port_input, 1, 1)
        onenet_push_layout.addWidget(QLabel("Token:"), 2, 0)
        onenet_push_layout.addWidget(self.push_token_input, 2, 1)
        onenet_push_layout.addWidget(QLabel("AES Key:"), 3, 0)
        onenet_push_layout.addWidget(self.push_aes_key_input, 3, 1)
        self.connection_stack.addWidget(self.onenet_push_widget)

        # --- 模式 6: MQTT Relay ---
        self.mqtt_relay_widget = QWidget()
        mqtt_relay_layout = QGridLayout(self.mqtt_relay_widget)
        mqtt_relay_layout.setContentsMargins(0,0,0,0)
        
        self.relay_host_input = QLineEdit("101.34.228.243")
        self.relay_port_input = QLineEdit("1883")
        self.relay_user_input = QLineEdit("admin")
        self.relay_pass_input = QLineEdit("onenet_mqtt_2024")
        self.relay_pass_input.setEchoMode(QLineEdit.EchoMode.Password)
        self.relay_topic_input = QLineEdit("onenet/data/push")

        mqtt_relay_layout.addWidget(QLabel("服务器 IP:"), 0, 0)
        mqtt_relay_layout.addWidget(self.relay_host_input, 0, 1)
        mqtt_relay_layout.addWidget(QLabel("端口:"), 1, 0)
        mqtt_relay_layout.addWidget(self.relay_port_input, 1, 1)
        mqtt_relay_layout.addWidget(QLabel("用户名:"), 2, 0)
        mqtt_relay_layout.addWidget(self.relay_user_input, 2, 1)
        mqtt_relay_layout.addWidget(QLabel("密码:"), 3, 0)
        mqtt_relay_layout.addWidget(self.relay_pass_input, 3, 1)
        mqtt_relay_layout.addWidget(QLabel("主题:"), 4, 0)
        mqtt_relay_layout.addWidget(self.relay_topic_input, 4, 1)
        self.connection_stack.addWidget(self.mqtt_relay_widget)
        
        self.connect_button = QPushButton("连接")
        self.connect_button.setCheckable(True)
        self.connect_button.clicked.connect(self.toggle_connection)
        
        conn_layout.addWidget(QLabel("模式:"), 0, 0)
        conn_layout.addWidget(self.mode_combo, 0, 1, 1, 2)
        conn_layout.addWidget(self.connection_stack, 1, 0, 1, 3)
        conn_layout.addWidget(self.connect_button, 2, 0, 1, 3)
        left_panel_layout.addWidget(conn_group)
        
        # -- 2. 创建选项卡 --
        left_tabs = QTabWidget()
        left_panel_layout.addWidget(left_tabs)

        # -- 控制选项卡 --
        control_tab = QWidget()
        control_tab_layout = QVBoxLayout(control_tab)

        # -- 基础控制 --
        manual_control_group = QGroupBox("手动控制")
        control_layout = QGridLayout(manual_control_group)
        
        btn_fwd = QPushButton("↑ 前进")
        btn_left = QPushButton("← 左转")
        btn_stop = QPushButton("停止")
        btn_right = QPushButton("→ 右转")
        btn_back = QPushButton("↓ 后退")

        # Connect buttons to send commands
        btn_fwd.clicked.connect(lambda: self.send_control_command(0x01))
        btn_left.clicked.connect(lambda: self.send_control_command(0x03))
        btn_stop.clicked.connect(lambda: self.send_control_command(0x00))
        btn_right.clicked.connect(lambda: self.send_control_command(0x04))
        btn_back.clicked.connect(lambda: self.send_control_command(0x02))

        control_layout.addWidget(btn_fwd, 0, 1)
        control_layout.addWidget(btn_left, 1, 0)
        control_layout.addWidget(btn_stop, 1, 1)
        control_layout.addWidget(btn_right, 1, 2)
        control_layout.addWidget(btn_back, 2, 1)

        # 在 control_group 中添加速度滑块
        speed_label = QLabel("速度 (%):")
        self.speed_slider = QSlider(Qt.Orientation.Horizontal)
        self.speed_slider.setRange(0, 100)
        self.speed_slider.setValue(50)
        self.speed_slider.setToolTip("调节小车速度 (0-100%)")
        self.speed_slider.valueChanged.connect(self.send_speed_command)
        
        control_layout.addWidget(speed_label, 3, 0)
        control_layout.addWidget(self.speed_slider, 3, 1, 1, 2)

        # 添加目标偏航角设置
        yaw_label = QLabel("目标偏航角 (°):")
        self.yaw_spinbox = QDoubleSpinBox()
        self.yaw_spinbox.setRange(-360.0, 360.0)
        self.yaw_spinbox.setValue(0.0)
        self.yaw_spinbox.setSingleStep(1.0)
        
        set_yaw_button = QPushButton("设置")
        set_yaw_button.clicked.connect(self.send_target_yaw_command)
        
        control_layout.addWidget(yaw_label, 4, 0)
        control_layout.addWidget(self.yaw_spinbox, 4, 1)
        control_layout.addWidget(set_yaw_button, 4, 2)
        control_tab_layout.addWidget(manual_control_group)

        # -- 手动发送指令 --
        self.manual_send_group = QGroupBox("手动发送 (串口)")
        manual_send_layout = QGridLayout(self.manual_send_group)

        self.manual_send_input = QLineEdit()
        self.manual_send_input.setPlaceholderText("输入指令 (HEX格式请用空格分隔, 如 AA 55)")
        manual_send_layout.addWidget(self.manual_send_input, 0, 0, 1, 2)

        self.manual_send_format_combo = QComboBox()
        self.manual_send_format_combo.addItems(["HEX", "ASCII"])
        manual_send_layout.addWidget(self.manual_send_format_combo, 1, 0)
        
        self.manual_send_button = QPushButton("发送")
        self.manual_send_button.clicked.connect(self.send_manual_command)
        manual_send_layout.addWidget(self.manual_send_button, 1, 1)
        
        control_tab_layout.addWidget(self.manual_send_group)

        # -- 自动导航 --
        nav_group = QGroupBox("自动导航")
        nav_layout = QGridLayout(nav_group)
        
        nav_layout.addWidget(QLabel("点击地图自动填充，或手动输入:"), 0, 0, 1, 3)

        nav_layout.addWidget(QLabel("目标纬度:"), 1, 0)
        self.dest_lat_spinbox = QDoubleSpinBox()
        self.dest_lat_spinbox.setRange(-90.0, 90.0)
        self.dest_lat_spinbox.setDecimals(6)
        self.dest_lat_spinbox.setSingleStep(0.0001)
        nav_layout.addWidget(self.dest_lat_spinbox, 1, 1, 1, 2)
        
        nav_layout.addWidget(QLabel("目标经度:"), 2, 0)
        self.dest_lon_spinbox = QDoubleSpinBox()
        self.dest_lon_spinbox.setRange(-180.0, 180.0)
        self.dest_lon_spinbox.setDecimals(6)
        self.dest_lon_spinbox.setSingleStep(0.0001)
        nav_layout.addWidget(self.dest_lon_spinbox, 2, 1, 1, 2)

        self.set_dest_button = QPushButton("设置目标")
        self.set_dest_button.clicked.connect(self.set_destination_from_ui)
        nav_layout.addWidget(self.set_dest_button, 3, 0, 1, 3)

        self.nav_button = QPushButton("开始导航")
        self.nav_button.setCheckable(True)
        self.nav_button.clicked.connect(self.toggle_navigation)
        self.nav_status_label = QLabel("状态: 未激活")
        
        nav_layout.addWidget(self.nav_button, 4, 0)
        nav_layout.addWidget(self.nav_status_label, 4, 1, 1, 2)
        control_tab_layout.addWidget(nav_group)
        control_tab_layout.addStretch()

        # -- 参数选项卡 --
        params_tab = QWidget()
        params_tab_layout = QVBoxLayout(params_tab)
        params_tab_layout.setAlignment(Qt.AlignmentFlag.AlignTop) # 内容顶部对齐

        # -- PID 调参 --
        pid_group = QGroupBox("PID 实时调参")
        pid_layout = QGridLayout(pid_group)

        pid_layout.addWidget(QLabel("KP (比例):"), 0, 0)
        self.kp_spinbox = QDoubleSpinBox()
        self.kp_spinbox.setRange(0.0, 10.0)
        self.kp_spinbox.setValue(0.8)
        self.kp_spinbox.setSingleStep(0.1)
        pid_layout.addWidget(self.kp_spinbox, 0, 1)

        pid_layout.addWidget(QLabel("KI (积分):"), 1, 0)
        self.ki_spinbox = QDoubleSpinBox()
        self.ki_spinbox.setRange(0.0, 10.0)
        self.ki_spinbox.setValue(0.2)
        self.ki_spinbox.setSingleStep(0.1)
        pid_layout.addWidget(self.ki_spinbox, 1, 1)

        pid_layout.addWidget(QLabel("KD (微分):"), 2, 0)
        self.kd_spinbox = QDoubleSpinBox()
        self.kd_spinbox.setRange(0.0, 10.0)
        self.kd_spinbox.setValue(0.1)
        self.kd_spinbox.setSingleStep(0.1)
        pid_layout.addWidget(self.kd_spinbox, 2, 1)

        set_pid_button = QPushButton("发送 PID 参数")
        set_pid_button.clicked.connect(self.send_pid_params_command)
        pid_layout.addWidget(set_pid_button, 3, 0, 1, 2)
        params_tab_layout.addWidget(pid_group)
        
        # -- 状态选项卡 --
        status_tab = QWidget()
        status_tab_layout = QVBoxLayout(status_tab)
        status_tab_layout.setAlignment(Qt.AlignmentFlag.AlignTop) # 内容顶部对齐

        # -- 实时数据 --
        data_group = QGroupBox("实时数据")

        # GPS Data
        data_layout = QGridLayout(data_group)

        data_layout.addWidget(QLabel("经度:"), 0, 0)
        self.lon_label = QLabel("N/A")
        data_layout.addWidget(self.lon_label, 0, 1)
        data_layout.addWidget(QLabel("纬度:"), 1, 0)
        self.lat_label = QLabel("N/A")
        data_layout.addWidget(self.lat_label, 1, 1)
        data_layout.addWidget(QLabel("GPS速度:"), 0, 2)
        self.gps_speed_label = QLabel("N/A")
        data_layout.addWidget(self.gps_speed_label, 0, 3)
        data_layout.addWidget(QLabel("卫星数:"), 1, 2)
        self.satellites_label = QLabel("N/A")
        data_layout.addWidget(self.satellites_label, 1, 3)
        
        # Attitude Data
        data_layout.addWidget(QLabel("Pitch:"), 2, 0)
        self.pitch_label = QLabel("N/A")
        data_layout.addWidget(self.pitch_label, 2, 1)
        data_layout.addWidget(QLabel("Roll:"), 2, 2)
        self.roll_label = QLabel("N/A")
        data_layout.addWidget(self.roll_label, 2, 3)
        data_layout.addWidget(QLabel("Yaw:"), 3, 0)
        self.yaw_label = QLabel("N/A")
        data_layout.addWidget(self.yaw_label, 3, 1)
        data_layout.addWidget(QLabel("航向角:"), 3, 2)
        self.heading_label = QLabel("N/A")
        data_layout.addWidget(self.heading_label, 3, 3)
        
        # Vehicle Status
        data_layout.addWidget(QLabel("左电机:"), 4, 0)
        self.motorL_label = QLabel("N/A")
        data_layout.addWidget(self.motorL_label, 4, 1)
        data_layout.addWidget(QLabel("右电机:"), 4, 2)
        self.motorR_label = QLabel("N/A")
        data_layout.addWidget(self.motorR_label, 4, 3)
        data_layout.addWidget(QLabel("电池电压:"), 5, 0)
        self.battery_label = QLabel("N/A")
        data_layout.addWidget(self.battery_label, 5, 1)

        # Status Lights
        status_indicator_layout = QHBoxLayout()
        self.mpu_status_light = self.create_status_light()
        self.hmc_status_light = self.create_status_light()
        self.gps_status_light = self.create_status_light()
        status_indicator_layout.addWidget(QLabel("MPU:"))
        status_indicator_layout.addWidget(self.mpu_status_light)
        status_indicator_layout.addWidget(QLabel("HMC:"))
        status_indicator_layout.addWidget(self.hmc_status_light)
        status_indicator_layout.addWidget(QLabel("GPS:"))
        status_indicator_layout.addWidget(self.gps_status_light)
        status_indicator_layout.addStretch()

        self.refresh_api_button = QPushButton("手动刷新")
        self.refresh_api_button.clicked.connect(self.poll_device_properties_via_api)
        self.refresh_api_button.setEnabled(False) # 默认禁用
        status_indicator_layout.addWidget(self.refresh_api_button)

        data_layout.addLayout(status_indicator_layout, 6, 0, 1, 4)
        status_tab_layout.addWidget(data_group)

        # -- 数据记录 --
        log_control_group = QGroupBox("数据记录")
        log_control_layout = QVBoxLayout(log_control_group)

        self.log_button = QPushButton("开始记录")
        self.log_button.setCheckable(True)
        self.log_button.clicked.connect(self.toggle_logging)

        self.log_status_label = QLabel("状态: 未记录")
        self.log_status_label.setWordWrap(True)

        log_control_layout.addWidget(self.log_button)
        log_control_layout.addWidget(self.log_status_label)
        status_tab_layout.addWidget(log_control_group)
        status_tab_layout.addStretch()

        # -- 将选项卡添加到主选项卡控件 --
        left_tabs.addTab(control_tab, "控制")
        left_tabs.addTab(params_tab, "参数")
        left_tabs.addTab(status_tab, "状态")

        left_panel_layout.addStretch()

        # =====================================================================
        # 右侧面板 (选项卡)
        # =====================================================================
        right_panel_tabs = QTabWidget()
        main_layout.addWidget(right_panel_tabs, stretch=1)

        # -- 地图选项卡 --
        map_tab = QWidget()
        map_tab_layout = QVBoxLayout(map_tab)
        map_controls_layout = QHBoxLayout()

        map_style_label = QLabel("地图样式:")
        self.map_style_combo = QComboBox()
        self.map_style_combo.addItems(["高德街道", "高德卫星", "OpenStreetMap"])
        self.map_style_combo.currentTextChanged.connect(self.change_map_layer)
        
        self.center_map_check = QCheckBox("锁定中心")
        self.center_map_check.setChecked(True)
        clear_traj_button = QPushButton("清除轨迹")
        clear_traj_button.clicked.connect(self.clear_trajectory)

        map_controls_layout.addWidget(map_style_label)
        map_controls_layout.addWidget(self.map_style_combo)
        map_controls_layout.addStretch()
        map_controls_layout.addWidget(self.center_map_check)
        map_controls_layout.addWidget(clear_traj_button)
        
        map_tab_layout.addWidget(self.map_view)
        map_tab_layout.addLayout(map_controls_layout)
        right_panel_tabs.addTab(map_tab, "地图与轨迹")
        
        # -- 曲线选项卡 --
        plot_tab = QWidget()
        self.plot_tab_layout = QGridLayout(plot_tab)

        # 创建 2x2 的绘图控件矩阵
        self.plot_widget_pitch = pg.PlotWidget(title="俯仰角 (Pitch)")
        self.plot_widget_roll = pg.PlotWidget(title="翻滚角 (Roll)")
        self.plot_widget_yaw = pg.PlotWidget(title="偏航角 (Yaw vs Target)")
        self.plot_widget_yaw_error = pg.PlotWidget(title="偏航角误差 (Yaw Error)")

        plot_widgets = [self.plot_widget_pitch, self.plot_widget_roll, self.plot_widget_yaw, self.plot_widget_yaw_error]
        for widget in plot_widgets:
            widget.setBackground('#3c3c3c')
            widget.showGrid(x=True, y=True)
            widget.setLabel('left', '角度 (°)')
            widget.setLabel('bottom', '时间点')
            widget.addLegend(offset=(-10, 10)) # 调整图例位置，避免遮挡

        # 将绘图控件添加到网格布局
        self.plot_tab_layout.addWidget(self.plot_widget_pitch, 0, 0)
        self.plot_tab_layout.addWidget(self.plot_widget_roll, 0, 1)
        self.plot_tab_layout.addWidget(self.plot_widget_yaw, 1, 0)
        self.plot_tab_layout.addWidget(self.plot_widget_yaw_error, 1, 1)

        # 在各自的控件上创建曲线
        self.plot_curve_pitch = self.plot_widget_pitch.plot(pen=pg.mkPen('#e74c3c', width=2), name="Pitch")
        self.plot_curve_roll = self.plot_widget_roll.plot(pen=pg.mkPen('#2ecc71', width=2), name="Roll")
        
        self.plot_curve_yaw = self.plot_widget_yaw.plot(pen=pg.mkPen('#3498db', width=2), name="Yaw")
        self.plot_curve_target_yaw = self.plot_widget_yaw.plot(pen=pg.mkPen('#f1c40f', width=2, style=Qt.PenStyle.DashLine), name="Target Yaw")
        
        self.plot_curve_yaw_error = self.plot_widget_yaw_error.plot(pen=pg.mkPen('#9b59b6', width=2), name="Yaw Error")

        pid_status_group = QGroupBox("PID 实时状态")
        pid_status_layout = QGridLayout(pid_status_group)
        pid_status_layout.addWidget(QLabel("目标偏航角:"), 0, 0)
        self.pid_target_yaw_label = QLabel("0.00°")
        pid_status_layout.addWidget(self.pid_target_yaw_label, 0, 1)
        pid_status_layout.addWidget(QLabel("当前偏航角:"), 1, 0)
        self.pid_current_yaw_label = QLabel("N/A")
        pid_status_layout.addWidget(self.pid_current_yaw_label, 1, 1)
        pid_status_layout.addWidget(QLabel("误差:"), 2, 0)
        self.pid_error_label = QLabel("N/A")
        pid_status_layout.addWidget(self.pid_error_label, 2, 1)
        
        # 将 PID 状态组添加到布局底部，并跨越两列
        self.plot_tab_layout.addWidget(pid_status_group, 2, 0, 1, 2)
        right_panel_tabs.addTab(plot_tab, "实时曲线")
        
        # -- 日志选项卡 --
        log_tab = QWidget()
        log_tab_layout = QVBoxLayout(log_tab)
        self.log_text = QTextEdit()
        self.log_text.setReadOnly(True)

        controls_layout = QHBoxLayout()
        clear_log_button = QPushButton("清除日志")
        clear_log_button.clicked.connect(self.log_text.clear)
        controls_layout.addWidget(clear_log_button)
        controls_layout.addStretch()

        log_tab_layout.addWidget(self.log_text)
        log_tab_layout.addLayout(controls_layout)

        # -- 新增：通用/手动发送框 --
        send_group = QGroupBox("发送数据")
        send_layout = QGridLayout(send_group)
        log_tab_layout.addWidget(send_group)
        
        self.universal_send_input = QLineEdit()
        self.universal_send_input.setPlaceholderText("云端模式: (JSON or text)")
        self.universal_send_button = QPushButton("发送")
        self.universal_send_button.clicked.connect(self.send_universal_command)
        send_layout.addWidget(QLabel("云端指令:"), 0, 0)
        send_layout.addWidget(self.universal_send_input, 0, 1)
        send_layout.addWidget(self.universal_send_button, 0, 2)
        
        self.manual_send_input = QLineEdit()
        self.manual_send_input.setPlaceholderText("串口模式: (HEX, e.g., AA 55)")
        self.manual_send_format_combo = QComboBox()
        self.manual_send_format_combo.addItems(["HEX", "ASCII"])
        self.manual_send_button = QPushButton("发送")
        self.manual_send_button.clicked.connect(self.send_manual_command)
        send_layout.addWidget(QLabel("串口指令:"), 1, 0)
        send_layout.addWidget(self.manual_send_input, 1, 1)
        send_layout.addWidget(self.manual_send_format_combo, 1, 2)
        send_layout.addWidget(self.manual_send_button, 1, 3)

        right_panel_tabs.addTab(log_tab, "通信日志")
        
    def create_status_light(self):
        light = QLabel()
        light.setFixedSize(20, 20)
        light.setStyleSheet("background-color: #555; border-radius: 10px; border: 1px solid #444;")
        return light
        
    def set_connection_widgets_enabled(self, enabled):
        """启用或禁用所有连接相关的设置控件。"""
        self.mode_combo.setEnabled(enabled)
        # Serial
        self.ports_combo.setEnabled(enabled)
        self.refresh_button.setEnabled(enabled)
        self.baudrate_combo.setEnabled(enabled)
        self.databits_combo.setEnabled(enabled)
        self.parity_combo.setEnabled(enabled)
        self.stopbits_combo.setEnabled(enabled)
        
        # 自定义TCP服务器
        self.tcp_server_input.setEnabled(enabled)
        self.tcp_port_input.setEnabled(enabled)
        self.tcp_device_id_input.setEnabled(enabled)
        
        # OneNet MQTT
        self.mqtt_product_id_input.setEnabled(enabled)
        self.mqtt_device_id_input.setEnabled(enabled)
        self.mqtt_token_input.setEnabled(enabled)
        # OneNet HTTP Push
        self.push_project_path_input.setEnabled(enabled)
        self.push_token_input.setEnabled(enabled)
        self.push_aes_key_input.setEnabled(enabled)
        self.push_port_input.setEnabled(enabled)
        # MQTT Relay
        self.relay_host_input.setEnabled(enabled)
        self.relay_port_input.setEnabled(enabled)
        self.relay_user_input.setEnabled(enabled)
        self.relay_pass_input.setEnabled(enabled)
        self.relay_topic_input.setEnabled(enabled)
        # Pulsar
        self.pulsar_project_path_input.setEnabled(enabled)
        # OneNet API Command
        self.api_cmd_user_id_input.setEnabled(enabled)
        self.api_cmd_access_key_input.setEnabled(enabled)
        self.api_cmd_product_id_input.setEnabled(enabled)
        self.api_cmd_device_name_input.setEnabled(enabled)

        # 控制发送框的可用性
        is_serial = (self.connection_mode == "串口")
        cloud_modes = ["OneNet MQTT", "OneNet API (指令)", "自定义TCP服务器"]
        is_cloud_send = (self.connection_mode in cloud_modes)
        
        self.manual_send_input.setEnabled(not enabled and is_serial)
        self.manual_send_format_combo.setEnabled(not enabled and is_serial)
        self.manual_send_button.setEnabled(not enabled and is_serial)
        
        self.universal_send_input.setEnabled(not enabled and is_cloud_send)
        self.universal_send_button.setEnabled(not enabled and is_cloud_send)

    def update_status_indicators(self):
        on_color = "#2ecc71"  # Green
        off_color = "#c0392b" # Red
        self.mpu_status_light.setStyleSheet(f"background-color: {on_color if self.system_status['mpu'] else off_color}; border-radius: 10px;")
        self.hmc_status_light.setStyleSheet(f"background-color: {on_color if self.system_status['hmc'] else off_color}; border-radius: 10px;")
        self.gps_status_light.setStyleSheet(f"background-color: {on_color if self.system_status['gps'] else off_color}; border-radius: 10px;")

    def update_connection_ui(self):
        self.connection_mode = self.mode_combo.currentText()
        is_serial = (self.connection_mode == "串口")
        cloud_modes = ["OneNet MQTT", "OneNet API (指令)", "自定义TCP服务器"]
        is_cloud_send = (self.connection_mode in cloud_modes)

        self.manual_send_input.setEnabled(is_serial)
        self.manual_send_format_combo.setEnabled(is_serial)
        self.manual_send_button.setEnabled(is_serial)
        
        self.universal_send_input.setEnabled(is_cloud_send)
        self.universal_send_button.setEnabled(is_cloud_send)

    def toggle_connection(self, checked):
        if checked:
            # Update button state first
            self.connect_button.setText("断开")
            
            if self.connection_mode == "串口":
                self.connect_serial()
            elif self.connection_mode == "自定义TCP服务器":
                self.connect_tcp()
            elif self.connection_mode == "OneNet MQTT":
                self.connect_onenet_mqtt()
            elif self.connection_mode == "OneNet API (指令)":
                self.connect_onenet_api_cmd()
            elif self.connection_mode == "Pulsar 数据流":
                self.connect_pulsar()
            elif self.connection_mode == "OneNet HTTP 推送":
                self.connect_onenet_http_push()
            else: # MQTT Relay
                self.connect_mqtt_relay()
        else:
            # Update button state first
            self.connect_button.setText("连接")
            self.disconnect()
            
    # connect_serial, disconnect, handle_data, process_buffer, process_packet ...
    # send_command, and other helpers...
    # The logic for these methods remains largely the same but needs to be adapted
    # to the new threading and connection mode structure.
    # For brevity, only showing the new/changed parts.

    def connect_serial(self):
        port_name = self.ports_combo.currentText()
        if "无可用串口" in port_name:
            self.log("连接失败: 没有可用的串口。", level='ERROR')
            self.connect_button.setChecked(False)
            return

        try:
            self.serial_port.port = port_name
            self.serial_port.baudrate = int(self.baudrate_combo.currentText())
            self.serial_port.bytesize = int(self.databits_combo.currentText())
            parity_map = {
                'None': serial.PARITY_NONE, 'Even': serial.PARITY_EVEN, 'Odd': serial.PARITY_ODD,
                'Mark': serial.PARITY_MARK, 'Space': serial.PARITY_SPACE
            }
            self.serial_port.parity = parity_map[self.parity_combo.currentText()]
            stopbits_map = {'1': serial.STOPBITS_ONE, '1.5': serial.STOPBITS_ONE_POINT_FIVE, '2': serial.STOPBITS_TWO}
            self.serial_port.stopbits = stopbits_map[self.stopbits_combo.currentText()]
            self.serial_port.timeout = 1
            self.serial_port.open()
            self.clear_plots()
            
            self.set_connection_widgets_enabled(False)
            self.manual_send_group.setEnabled(True)

            self.comm_worker = SerialWorker(self.serial_port)
            self.comm_thread = QThread()
            self.comm_worker.moveToThread(self.comm_thread)
            
            self.comm_worker.log_message.connect(self.log)
            self.comm_worker.data_received.connect(self.handle_data)
            self.comm_thread.started.connect(self.comm_worker.run)
            self.comm_thread.finished.connect(self.on_thread_finished)
            
            self.comm_thread.start()
            
            self.log(f"成功连接到 {port_name}", level='SUCCESS')

        except Exception as e:
            self.log(f"连接失败: {e}", level='ERROR')
            self.connect_button.setChecked(False)

    def connect_tcp(self):
        """
        连接到自定义TCP服务器
        
        从UI获取服务器地址、端口和设备ID
        创建TCP工作线程并启动
        设置信号连接以处理数据和日志
        """
        server_address = self.tcp_server_input.text()
        port = self.tcp_port_input.text()
        device_id = self.tcp_device_id_input.text()

        if not server_address or not port or not device_id:
            self.log("TCP连接失败: 服务器地址、端口和设备ID不能为空。", level='ERROR')
            self.connect_button.setChecked(False)
            return

        self.clear_plots()
        self.set_connection_widgets_enabled(False)

        self.comm_worker = CustomTcpWorker(server_address, port, device_id)
        self.comm_thread = QThread()
        self.comm_worker.moveToThread(self.comm_thread)

        self.comm_worker.log_message.connect(self.log)
        self.comm_worker.data_received.connect(self.handle_tcp_data)
        self.comm_thread.started.connect(self.comm_worker.run)
        self.comm_thread.finished.connect(self.on_thread_finished)

        self.comm_thread.start()
        self.log(f"正在连接到TCP服务器 {server_address}:{port}...", level='SUCCESS')

    def connect_onenet(self):
        api_key = self.onenet_api_key_input.text()
        device_id = self.onenet_device_id_input.text()
        if not api_key or not device_id:
            self.log("OneNet连接失败: API Key和Device ID不能为空。", level='ERROR')
            self.connect_button.setChecked(False)
            return

        self.clear_plots()
        self.set_connection_widgets_enabled(False)
        self.comm_worker = OneNetWorker(api_key, device_id)
        self.comm_thread = QThread()
        self.comm_worker.moveToThread(self.comm_thread)
        
        self.comm_worker.log_message.connect(self.log)
        self.comm_worker.data_received.connect(self.handle_onenet_data)
        self.comm_thread.started.connect(self.comm_worker.run)
        self.comm_thread.finished.connect(self.on_thread_finished)
        
        self.comm_thread.start()
        self.log(f"正在通过MQTT连接到OneNet平台...", level='SUCCESS')

    def connect_onenet_mqtt(self):
        product_id = self.mqtt_product_id_input.text()
        device_id = self.mqtt_device_id_input.text()
        token = self.mqtt_token_input.text()
        
        if not product_id or not device_id or not token:
            self.log("MQTT连接失败: 产品ID、设备ID和Token不能为空。", level='ERROR')
            self.connect_button.setChecked(False)
            return

        self.clear_plots()
        self.set_connection_widgets_enabled(False)
        
        self.comm_worker = OneNetMQTTWorker(product_id, device_id, token)
        self.comm_thread = QThread()
        self.comm_worker.moveToThread(self.comm_thread)
        
        self.comm_worker.log_message.connect(self.log)
        self.comm_worker.data_received.connect(self.handle_mqtt_data)
        self.comm_thread.started.connect(self.comm_worker.run)
        self.comm_thread.finished.connect(self.on_thread_finished)
        
        self.comm_thread.start()
        self.log(f"正在通过MQTT连接到OneNet平台...", level='SUCCESS')

    def connect_pulsar(self):
        project_path = self.pulsar_project_path_input.text()
        if not project_path or not os.path.isdir(project_path):
            self.log(f"Pulsar连接失败: 无效的项目路径 '{project_path}'。", level='ERROR')
            self.connect_button.setChecked(False)
            return

        self.clear_plots()
        self.set_connection_widgets_enabled(False)

        self.comm_worker = PulsarWorker(project_path)
        self.comm_thread = QThread()
        self.comm_worker.moveToThread(self.comm_thread)

        self.comm_worker.log_message.connect(self.log)
        self.comm_worker.data_received.connect(self.handle_pulsar_data)
        self.comm_thread.started.connect(self.comm_worker.run)
        self.comm_thread.finished.connect(self.on_thread_finished)

        self.comm_thread.start()
        self.log("启动Pulsar Java客户端...", level='SUCCESS')

    def connect_onenet_http_push(self):
        project_path = self.push_project_path_input.text()
        token = self.push_token_input.text()
        aes_key = self.push_aes_key_input.text()
        port = self.push_port_input.text()

        if not project_path or not os.path.isdir(project_path):
            self.log(f"HTTP Push连接失败: 无效的项目路径 '{project_path}'。", level='ERROR')
            self.connect_button.setChecked(False)
            return
        if not token or not port:
            self.log("HTTP Push连接失败: Token和端口号不能为空。", level='ERROR')
            self.connect_button.setChecked(False)
            return

        self.clear_plots()
        self.set_connection_widgets_enabled(False)

        self.comm_worker = OneNetHttpPushWorker(project_path, token, aes_key, port)
        self.comm_thread = QThread()
        self.comm_worker.moveToThread(self.comm_thread)

        self.comm_worker.log_message.connect(self.log)
        self.comm_worker.data_received.connect(self.handle_onenet_push_data)
        self.comm_thread.started.connect(self.comm_worker.run)
        self.comm_thread.finished.connect(self.on_thread_finished)

        self.comm_thread.start()
        self.log(f"启动OneNet HTTP Push服务在端口 {port}...", level='SUCCESS')

    def connect_mqtt_relay(self):
        host = self.relay_host_input.text()
        port = self.relay_port_input.text()
        user = self.relay_user_input.text()
        password = self.relay_pass_input.text()
        topic = self.relay_topic_input.text()

        if not all([host, port, topic]):
            self.log("MQTT Relay连接失败: 服务器IP、端口和主题不能为空。", level='ERROR')
            self.connect_button.setChecked(False)
            return

        self.clear_plots()
        self.set_connection_widgets_enabled(False)

        self.comm_worker = MqttRelayWorker(host, port, user, password, topic)
        self.comm_thread = QThread()
        self.comm_worker.moveToThread(self.comm_thread)

        self.comm_worker.log_message.connect(self.log)
        self.comm_worker.data_received.connect(self.handle_relay_data)
        self.comm_thread.started.connect(self.comm_worker.run)
        self.comm_thread.finished.connect(self.on_thread_finished)
        
        self.comm_thread.start()
        self.log(f"正在连接到云服务中继 {host}:{port}...", level='SUCCESS')

    def disconnect(self):
        if self.comm_worker:
            self.comm_worker.stop()
        
        if self.comm_thread and self.comm_thread.isRunning():
            self.comm_thread.quit()
            self.comm_thread.wait(1000) # Wait for thread to finish
        
        if self.serial_port.is_open:
            self.serial_port.close()
            
        if self.api_poll_timer.isActive():
            self.api_poll_timer.stop()
            
        self.api_cmd_config = {'user_id': '', 'access_key': '', 'product_id': '', 'device_name': ''}
        self.refresh_api_button.setEnabled(False)
        self.log("已断开连接")
        self.set_connection_widgets_enabled(True) 
        self.update_connection_ui()
        # UI state for button is handled in toggle_connection now
        if not self.connect_button.isChecked():
            self.connect_button.setText("连接")

    def on_thread_finished(self):
        self.log("通信线程已终止。")
        self.disconnect()

    @pyqtSlot(float, float)
    def handle_new_destination(self, lat, lon):
        """当地图被点击时，用坐标填充输入框以供预览。"""
        self.dest_lat_spinbox.setValue(lat)
        self.dest_lon_spinbox.setValue(lon)
        self.log(f"坐标点预览: Lat={lat:.6f}, Lon={lon:.6f}", level='INFO')

    def set_destination_from_ui(self):
        """从UI输入框读取坐标并设置为最终目标点。"""
        lat = self.dest_lat_spinbox.value()
        lon = self.dest_lon_spinbox.value()
        
        if abs(lat) < 1e-9 and abs(lon) < 1e-9:
            self.log("无效的目标点坐标。", level='ERROR')
            return

        self.destination = (lat, lon)
        self.log(f"新目标点已设置: Lat={lat:.6f}, Lon={lon:.6f}", level='SUCCESS')
        
        # 在地图上显示标记
        js_call = f"setDestinationMarker({lat}, {lon});"
        self.map_view.page().runJavaScript(js_call)
        
        # 如果正在导航中，立即更新航向
        if self.nav_timer.isActive():
            self.navigate_to_destination()

    @pyqtSlot(bytes)
    def handle_data(self, data):
        """Append incoming data to buffer and process it."""
        self.rx_buffer.extend(data)
        self.process_buffer()

    @pyqtSlot(dict)
    def handle_onenet_data(self, streams):
        self.log(f"ONENET_RX: {streams}", level='RX')
        processed_data = {}
        # OneNet HTTP API 返回的是数据流列表, 每个元素是一个字典
        key_map = { 
            'lon': 'lon', 'lat': 'lat', 'gps_speed': 'gps_speed', 'satellites': 'satellites',
            'pitch': 'pitch', 'roll': 'roll', 'yaw': 'yaw', 'heading': 'heading',
            'motor_l': 'motorL', 'motor_r': 'motorR', 'battery': 'battery',
            'mpu_status': 'mpu', 'hmc_status': 'hmc', 'gps_status': 'gps'
        }
        
        for stream in streams:
            onenet_key = stream.get('id')
            value = stream.get('current_value')
            if onenet_key in key_map and value is not None:
                internal_key = key_map[onenet_key]
                processed_data[internal_key] = value
                
        if processed_data:
            self.update_ui_with_data(processed_data)

    def process_buffer(self):
        """Parse the buffer for complete and valid packets."""
        # Frame structure: 0xAA 0x55 (Header) + CMD (1) + LEN (1) + DATA (N) + CHECKSUM (1)
        HEADER = b'\xAA\x55'
        MIN_PACKET_LEN = 5  # Header(2) + Cmd(1) + Len(1) + Checksum(1)

        while len(self.rx_buffer) >= MIN_PACKET_LEN:
            try:
                header_index = self.rx_buffer.find(HEADER)
                if header_index == -1:
                    # No header found, clear buffer to prevent overflow with bad data
                    self.rx_buffer.clear()
                    return

                # If header is not at the start, discard the bytes before it
                if header_index > 0:
                    self.log(f"Discarding {header_index} invalid bytes: {self.rx_buffer[:header_index].hex(' ').upper()}")
                    del self.rx_buffer[:header_index]

                # We have a potential packet starting with the header. Check if buffer is long enough for header and length byte.
                if len(self.rx_buffer) < 4:
                    return # Not enough data for header + cmd + len

                cmd = self.rx_buffer[2]
                data_len = self.rx_buffer[3]
                packet_len = MIN_PACKET_LEN + data_len

                if len(self.rx_buffer) < packet_len:
                    # Not enough data for the full packet yet
                    return

                # We have a full potential packet, let's extract it
                packet = self.rx_buffer[:packet_len]
                del self.rx_buffer[:packet_len]  # Consume packet from buffer

                # Validate checksum
                payload_for_checksum = packet[2:-1]  # CMD + LEN + DATA
                calculated_checksum = sum(payload_for_checksum) & 0xFF
                received_checksum = packet[-1]

                if calculated_checksum == received_checksum:
                    # Packet is valid, process it
                    self.process_packet(cmd, packet[4:-1])  # Pass CMD and DATA
                else:
                    self.log(f"Checksum ERROR! Packet: {packet.hex(' ').upper()}. Calc: {calculated_checksum:02X}, Recv: {received_checksum:02X}", level='ERROR')

            except IndexError as e:
                self.log(f"Error processing buffer: {e}. Buffer: {self.rx_buffer.hex(' ').upper()}", level='ERROR')
                self.rx_buffer.clear() # Clear buffer on error to recover

    def clear_plots(self):
        """Clears all data from the plots."""
        self.time_counter = 0
        self.time_points.clear()
        for key in self.plot_data:
            self.plot_data[key].clear()
        
        self.plot_curve_pitch.setData([], [])
        self.plot_curve_roll.setData([], [])
        self.plot_curve_yaw.setData([], [])
        self.plot_curve_target_yaw.setData([], [])
        self.plot_curve_yaw_error.setData([], [])
        self.log("实时曲线已清空。")

    def process_packet(self, cmd, data):
        """解析串口数据包并通过通用函数更新UI。"""
        self.log(f"RX: CMD={cmd:02X}, Data={data.hex(' ').upper()}", level='RX')
        
        update_dict = {}
        try:
            if cmd == 0x10: # GPS Data
                lon, lat, speed, sats = struct.unpack('<fffB', data)
                update_dict = {'lon': lon, 'lat': lat, 'gps_speed': speed, 'satellites': sats}
            
            elif cmd == 0x11: # IMU Data
                pitch, roll, yaw = struct.unpack('<fff', data)
                update_dict = {'pitch': pitch, 'roll': roll, 'yaw': yaw}
            
            elif cmd == 0x12: # Magnetometer Data
                heading, = struct.unpack('<f', data)
                update_dict = {'heading': heading}
            
            elif cmd == 0x13: # Vehicle Status
                motorL, motorR, battery = struct.unpack('<hhf', data)
                update_dict = {'motorL': motorL, 'motorR': motorR, 'battery': battery}
            
            elif cmd == 0x01: # System Heartbeat
                mpu, hmc, gps = struct.unpack('<BBB', data)
                update_dict = {'mpu': mpu, 'hmc': hmc, 'gps': gps}
            
            elif cmd == 0xEE: # Debug Log
                message = data.decode('utf-8')
                self.log(f"{message}", level='FIRMWARE')
            
            else:
                self.log(f"Unknown CMD: {cmd:02X}", level='ERROR')
                
            if update_dict:
                self.update_ui_with_data(update_dict)

        except (struct.error, UnicodeDecodeError) as e:
            self.log(f"串口包解析错误 CMD {cmd:02X}: {e}. Data: {data.hex(' ').upper()}", level='ERROR')

    def update_ui_with_data(self, data_dict):
        """通用UI更新函数，处理来自任何来源的数据字典。"""
        # GPS and Map
        if 'lat' in data_dict and 'lon' in data_dict:
            lat, lon = data_dict['lat'], data_dict['lon']
            self.lon_label.setText(f"{lon:.6f}")
            self.lat_label.setText(f"{lat:.6f}")
            self.latest_data.update({'lat': lat, 'lon': lon})
            
            # Determine GPS status based on satellites if available
            sats = data_dict.get('satellites', self.latest_data.get('satellites', 0))
            gps_status = 'good' if sats >= 4 else 'weak'
            self.update_map(lat, lon, gps_status)

        if 'gps_speed' in data_dict:
            speed = data_dict['gps_speed']
            self.gps_speed_label.setText(f"{speed:.2f} km/h")
            self.latest_data['gps_speed'] = speed

        if 'satellites' in data_dict:
            sats = data_dict['satellites']
            self.satellites_label.setText(f"{sats}")
            self.latest_data['satellites'] = sats

        # Attitude and Plots
        if any(k in data_dict for k in ['pitch', 'roll', 'yaw']):
            pitch = data_dict.get('pitch', self.latest_data['pitch'])
            roll = data_dict.get('roll', self.latest_data['roll'])
            yaw = data_dict.get('yaw', self.latest_data['yaw'])
            
            self.pitch_label.setText(f"{pitch:.2f}°")
            self.roll_label.setText(f"{roll:.2f}°")
            self.yaw_label.setText(f"{yaw:.2f}°")
            self.latest_data.update({'pitch': pitch, 'roll': roll, 'yaw': yaw})

            yaw_error = self.target_yaw - yaw
            self.pid_current_yaw_label.setText(f"{yaw:.2f}°")
            self.pid_error_label.setText(f"{yaw_error:.2f}°")

            # Update plot data
            self.time_points.append(self.time_counter)
            self.plot_data['pitch'].append(pitch)
            self.plot_data['roll'].append(roll)
            self.plot_data['yaw'].append(yaw)
            self.plot_data['target_yaw'].append(self.target_yaw)
            self.plot_data['yaw_error'].append(yaw_error)
            self.time_counter += 1

            # Limit data points to avoid performance issues
            max_points = 300
            if len(self.time_points) > max_points:
                self.time_points.pop(0)
                for key in self.plot_data:
                    if self.plot_data[key]: self.plot_data[key].pop(0)
            
            # Update curves
            self.plot_curve_pitch.setData(self.time_points, self.plot_data['pitch'])
            self.plot_curve_roll.setData(self.time_points, self.plot_data['roll'])
            self.plot_curve_yaw.setData(self.time_points, self.plot_data['yaw'])
            self.plot_curve_target_yaw.setData(self.time_points, self.plot_data['target_yaw'])
            self.plot_curve_yaw_error.setData(self.time_points, self.plot_data['yaw_error'])

        # Other data
        if 'heading' in data_dict:
            heading = data_dict['heading']
            self.heading_label.setText(f"{heading:.2f}°")
            self.latest_data['heading'] = heading
        
        if 'motorL' in data_dict and 'motorR' in data_dict:
            motorL, motorR = data_dict['motorL'], data_dict['motorR']
            self.motorL_label.setText(f"{motorL}")
            self.motorR_label.setText(f"{motorR}")
            self.latest_data.update({'motorL': motorL, 'motorR': motorR})

        if 'battery' in data_dict:
            battery = data_dict['battery']
            self.battery_label.setText(f"{battery:.2f} V")
            self.latest_data['battery'] = battery
        
        # System Status
        if any(k in data_dict for k in ['mpu', 'hmc', 'gps']):
            self.system_status['mpu'] = int(data_dict.get('mpu', self.system_status['mpu']))
            self.system_status['hmc'] = int(data_dict.get('hmc', self.system_status['hmc']))
            self.system_status['gps'] = int(data_dict.get('gps', self.system_status['gps']))
            self.update_status_indicators()

        self.write_log_entry() # Log every time new data comes in


    def send_command(self, cmd, data=b''):
        """Constructs and sends a packet. Only works in serial mode."""
        if self.connection_mode != "串口" or not self.serial_port or not self.serial_port.is_open:
            if self.connection_mode != "串口":
                 self.log("命令发送失败: 控制指令仅在 [串口] 模式下可用。", level='ERROR')
            else:
                 self.log("命令发送失败: 串口未连接。", level='ERROR')
            return

        data_len = len(data)
        if data_len > 255:
            self.log(f"Error: Data payload too large ({data_len} bytes).", level='ERROR')
            return
            
        packet = bytearray([0xAA, 0x55, cmd, data_len])
        packet.extend(data)
        
        # Calculate checksum on CMD, LEN, and DATA
        checksum = sum(packet[2:]) & 0xFF
        packet.append(checksum)
        
        try:
            self.serial_port.write(packet)
            self.log(f"TX: CMD={cmd:02X}, Data={packet[4:-1].hex(' ').upper()}", level='TX')
        except Exception as e:
            self.log(f"Failed to send command: {e}", level='ERROR')

    def send_control_command(self, control_code):
        """Helper to send a movement control command."""
        # Command 0x80 for motor control, data is a single byte for action
        if self.connection_mode == "串口":
            self.send_command(0x80, control_code.to_bytes(1, 'little'))
        elif self.connection_mode == "OneNet MQTT":
            if self.comm_worker and hasattr(self.comm_worker, 'send_data'):
                # 假设物模型中用于方向控制的标识符是 'direction'
                payload = json.dumps({"direction": control_code})
                self.comm_worker.send_data(payload)
                self.log(f"通过MQTT发送控制指令: {payload}")
            else:
                self.log("MQTT命令发送失败: 未连接。", level='ERROR')
        elif self.connection_mode == "自定义TCP服务器":
            if self.comm_worker and hasattr(self.comm_worker, 'send_control_command'):
                # 映射控制代码到动作名称
                action_map = {
                    0x00: "stop",
                    0x01: "forward",
                    0x02: "backward",
                    0x03: "turn_left",
                    0x04: "turn_right"
                }
                action = action_map.get(control_code, "unknown")
                self.comm_worker.send_control_command("move", {"direction": action})
                self.log(f"通过TCP发送控制指令: 动作={action}")
            else:
                self.log("TCP命令发送失败: 未连接。", level='ERROR')
        elif self.connection_mode == "OneNet API (指令)":
            payload = {"direction": control_code}
            self.send_command_via_api(json.dumps(payload))
        else:
            self.log(f"命令发送失败: 当前模式 [{self.connection_mode}] 不支持该指令。", level='ERROR')

    def send_speed_command(self, speed_percent):
        """Helper to send a speed set command (CMD 0x81)."""
        if self.connection_mode == "串口":
            self.send_command(0x81, speed_percent.to_bytes(1, 'little'))
            self.log(f"Sent speed command: {speed_percent}%")
        elif self.connection_mode == "OneNet MQTT":
            if self.comm_worker and hasattr(self.comm_worker, 'send_data'):
                # 假设物模型中用于速度控制的标识符是 'speed'
                payload = json.dumps({"speed": speed_percent})
                self.comm_worker.send_data(payload)
                self.log(f"通过MQTT发送速度指令: {payload}")
            else:
                self.log("MQTT命令发送失败: 未连接。", level='ERROR')
        elif self.connection_mode == "自定义TCP服务器":
            if self.comm_worker and hasattr(self.comm_worker, 'send_control_command'):
                self.comm_worker.send_control_command("set_speed", {"speed": speed_percent})
                self.log(f"通过TCP发送速度指令: 速度={speed_percent}%")
            else:
                self.log("TCP命令发送失败: 未连接。", level='ERROR')
        elif self.connection_mode == "OneNet API (指令)":
            payload = {"speed": speed_percent}
            self.send_command_via_api(json.dumps(payload))
        else:
            self.log(f"命令发送失败: 当前模式 [{self.connection_mode}] 不支持该指令。", level='ERROR')

    def send_target_yaw_command(self):
        """Helper to send a target yaw set command (CMD 0x82)."""
        self.target_yaw = self.yaw_spinbox.value()
        self.pid_target_yaw_label.setText(f"{self.target_yaw:.2f}°")
        
        if self.connection_mode == "串口":
            data = struct.pack('<f', self.target_yaw)
            self.send_command(0x82, data)
            self.log(f"Sent target yaw: {self.target_yaw:.2f}°")
        elif self.connection_mode == "OneNet MQTT":
            if self.comm_worker and hasattr(self.comm_worker, 'send_data'):
                # 假设物模型中用于目标偏航角的标识符是 'target_yaw'
                payload = json.dumps({"target_yaw": self.target_yaw})
                self.comm_worker.send_data(payload)
                self.log(f"通过MQTT发送目标偏航角: {payload}")
            else:
                self.log("MQTT命令发送失败: 未连接。", level='ERROR')
        elif self.connection_mode == "自定义TCP服务器":
            if self.comm_worker and hasattr(self.comm_worker, 'send_control_command'):
                self.comm_worker.send_control_command("set_yaw", {"target_yaw": self.target_yaw})
                self.log(f"通过TCP发送目标偏航角: {self.target_yaw:.2f}°")
            else:
                self.log("TCP命令发送失败: 未连接。", level='ERROR')
        elif self.connection_mode == "OneNet API (指令)":
            payload = {"target_yaw": self.target_yaw}
            self.send_command_via_api(json.dumps(payload))
        else:
            self.log(f"命令发送失败: 当前模式 [{self.connection_mode}] 不支持该指令。", level='ERROR')

    def send_pid_params_command(self):
        """Helper to send PID parameters (CMD 0x83)."""
        kp = self.kp_spinbox.value()
        ki = self.ki_spinbox.value()
        kd = self.kd_spinbox.value()
        
        if self.connection_mode == "串口":
            data = struct.pack('<fff', kp, ki, kd)
            self.send_command(0x83, data)
            self.log(f"Sent PID params: KP={kp:.2f}, KI={ki:.2f}, KD={kd:.2f}")
        elif self.connection_mode == "OneNet MQTT":
            if self.comm_worker and hasattr(self.comm_worker, 'send_data'):
                # 假设物模型中有独立的 'kp', 'ki', 'kd' 标识符
                payload = json.dumps({"kp": kp, "ki": ki, "kd": kd})
                self.comm_worker.send_data(payload)
                self.log(f"通过MQTT发送PID参数: {payload}")
            else:
                self.log("MQTT命令发送失败: 未连接。", level='ERROR')
        elif self.connection_mode == "自定义TCP服务器":
            if self.comm_worker and hasattr(self.comm_worker, 'send_control_command'):
                self.comm_worker.send_control_command("set_pid", {"kp": kp, "ki": ki, "kd": kd})
                self.log(f"通过TCP发送PID参数: KP={kp:.2f}, KI={ki:.2f}, KD={kd:.2f}")
            else:
                self.log("TCP命令发送失败: 未连接。", level='ERROR')
        elif self.connection_mode == "OneNet API (指令)":
            payload = {"kp": kp, "ki": ki, "kd": kd}
            self.send_command_via_api(json.dumps(payload))
        else:
            self.log(f"命令发送失败: 当前模式 [{self.connection_mode}] 不支持该指令。", level='ERROR')

    def send_manual_command(self):
        """发送手动输入的串口指令。"""
        if not self.serial_port or not self.serial_port.is_open:
            self.log("无法发送: 串口未连接。", level='ERROR')
            return

        command_str = self.manual_send_input.text()
        if not command_str:
            return

        send_format = self.manual_send_format_combo.currentText()
        
        try:
            if send_format == "HEX":
                # 移除多余空格并转换为字节
                parts = command_str.strip().split()
                data_to_send = bytes([int(part, 16) for part in parts])
            else:  # ASCII
                data_to_send = command_str.encode('utf-8')

            self.serial_port.write(data_to_send)
            self.log(f"MANUAL_TX: {data_to_send.hex(' ').upper()}", level='TX')
        except ValueError:
            self.log(f"HEX格式错误: '{command_str}' 包含无效的十六进制字符或格式。", level='ERROR')
        except Exception as e:
            self.log(f"发送手动指令失败: {e}", level='ERROR')

    def log(self, message, level='INFO'):
        """Logs a message to the text edit with timestamp and color."""
        color_map = {
            'INFO': '#f0f0f0',      # Default (light gray)
            'TX': '#00ffff',        # Cyan
            'RX': '#32cd32',        # Lime Green
            'ERROR': '#ff4757',     # Red
            'FIRMWARE': '#ff00ff',  # Magenta
            'SUCCESS': '#2ed573'    # Green
        }
        
        timestamp = datetime.now().strftime("%H:%M:%S.%f")[:-3]
        
        log_entry = f"[{timestamp}] [{level.upper()}] {message}"
        
        # Set color and append
        color = QColor(color_map.get(level, '#f0f0f0'))
        self.log_text.setTextColor(color)
        self.log_text.append(log_entry)
    
    def toggle_logging(self, checked):
        if checked:
            # Start logging
            try:
                timestamp = datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
                log_filename = f"log_{timestamp}.csv"
                log_filepath = os.path.join(os.path.dirname(__file__), log_filename)

                self.log_file = open(log_filepath, 'w', newline='', encoding='utf-8')
                self.csv_writer = csv.writer(self.log_file)
                
                # Write header
                header = ['timestamp', 'latitude', 'longitude', 'gps_speed', 'satellites', 'pitch', 'roll', 'yaw', 'heading', 'motorL', 'motorR', 'battery']
                self.csv_writer.writerow(header)
                
                self.is_logging = True
                self.log_button.setText("停止记录")
                self.log_status_label.setText(f"记录中:\n{log_filename}")
                self.log(f"开始记录日志到 {log_filename}", level='SUCCESS')
            except Exception as e:
                self.log(f"创建日志文件失败: {e}", level='ERROR')
                self.log_button.setChecked(False)
        else:
            # Stop logging
            if self.is_logging:
                self.is_logging = False
                if self.log_file:
                    self.log_file.close()
                    self.log_file = None
                self.csv_writer = None
                self.log_button.setText("开始记录")
                self.log_status_label.setText("状态: 未记录")
                self.log("日志记录已停止。")

    def write_log_entry(self):
        if not self.is_logging or not self.csv_writer:
            return
        
        try:
            timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
            row = [
                timestamp,
                self.latest_data['lat'],
                self.latest_data['lon'],
                self.latest_data['gps_speed'],
                self.latest_data['satellites'],
                self.latest_data['pitch'],
                self.latest_data['roll'],
                self.latest_data['yaw'],
                self.latest_data['heading'],
                self.latest_data['motorL'],
                self.latest_data['motorR'],
                self.latest_data['battery']
            ]
            self.csv_writer.writerow(row)
        except Exception as e:
            self.log(f"写入日志失败: {e}", level='ERROR')

    def update_map(self, lat, lon, gps_status='good'):
        """通过JS调用高效更新地图"""
        if not self.map_ready or (lat == 0.0 and lon == 0.0):
            return

        self.trajectory.append((lat, lon))
        heading = self.latest_data.get('heading', 0.0) # 使用航向角

        # 通过 runJavaScript 调用函数
        js_call = f"updateTrajectory({lat}, {lon}, {heading}, '{gps_status}');"
        self.map_view.page().runJavaScript(js_call)
        
        if self.center_map_check.isChecked():
            self.map_view.page().runJavaScript(f"centerMap({lat}, {lon});")

    def update_ports(self):
        self.ports_combo.clear()
        ports = serial.tools.list_ports.comports()
        if not ports:
            self.ports_combo.addItem("无可用串口")
        else:
            for port in sorted(ports, key=lambda p: p.device):
                self.ports_combo.addItem(port.device, port.description)

    def init_map(self):
        """使用Leaflet.js的HTML初始化WebEngineView。"""
        # 使用自定义页面以捕获JS控制台日志
        page = WebEnginePage(self.map_view)
        self.map_view.setPage(page)

        # 设置 WebChannel
        self.channel = QWebChannel(page)
        self.map_api = MapApi(self)
        self.map_api.map_clicked.connect(self.handle_new_destination)
        self.channel.registerObject("qt_api", self.map_api)
        page.setWebChannel(self.channel)

        self.map_view.loadFinished.connect(self.on_map_load_finished)
        self.map_view.setHtml(MAP_HTML, QUrl("qrc:///"))
        self.log("正在加载地图引擎...")

    def on_map_load_finished(self, success):
        """地图页面加载完成后的回调。"""
        if success:
            self.map_ready = True
            self.log("地图引擎加载成功，可以接收数据。", level='SUCCESS')
        else:
            self.map_ready = False
            self.log("地图引擎加载失败。", level='ERROR')

    def change_map_layer(self, layer_name):
        """切换地图瓦片图层。"""
        if self.map_ready:
            self.map_view.page().runJavaScript(f"changeTileLayer('{layer_name}');")

    def clear_trajectory(self):
        self.trajectory.clear()
        if self.map_ready:
            self.map_view.page().runJavaScript("clearTrajectory();")
        self.log("地图轨迹已清除。")
        
    def toggle_navigation(self, checked):
        if checked:
            if self.destination is None:
                self.log("导航启动失败: 请先手动输入或在地图上选择坐标，然后点击'设置目标'。", level='ERROR')
                self.nav_button.setChecked(False)
                return
            
            # 发送指令进入自动导航模式
            if self.connection_mode == "串口":
                self.send_command(0x84, b'\x01') 
                self.log("发送指令: 进入自动导航模式。", level='TX')
            elif self.connection_mode == "OneNet MQTT":
                if self.comm_worker and hasattr(self.comm_worker, 'send_data'):
                    # 假设物模型中用于导航模式的标识符是 'nav_mode'
                    payload = json.dumps({"nav_mode": 1})
                    self.comm_worker.send_data(payload)
                    self.log(f"通过MQTT发送指令: 进入导航模式")
                else:
                    self.log("MQTT命令发送失败: 未连接。", level='ERROR')
                    self.nav_button.setChecked(False)
                    return
            elif self.connection_mode == "自定义TCP服务器":
                if self.comm_worker and hasattr(self.comm_worker, 'send_control_command'):
                    self.comm_worker.send_control_command("set_navigation", {"mode": 1})
                    self.log("通过TCP发送指令: 进入导航模式")
                else:
                    self.log("TCP命令发送失败: 未连接。", level='ERROR')
                    self.nav_button.setChecked(False)
                    return
            elif self.connection_mode == "OneNet API (指令)":
                payload = {"nav_mode": 1}
                self.send_command_via_api(json.dumps(payload))
            else:
                self.log("导航启动失败: 控制指令仅在[串口]、[OneNet MQTT]或[自定义TCP服务器]模式下可用。", "ERROR")
                self.nav_button.setChecked(False)
                return

            self.nav_timer.start()
            self.nav_button.setText("停止导航")
            self.nav_status_label.setText("状态: 导航中...")
            self.log("自动导航已启动。", level='SUCCESS')
            self.navigate_to_destination() # 立即执行一次
        else:
            self.nav_timer.stop()
            
            # 发送指令退出自动导航模式
            if self.connection_mode == "串口":
                self.send_command(0x84, b'\x00')
                self.log("发送指令: 退出自动导航模式。", level='TX')
                # (可选) 发送停止指令，确保车辆停止
                self.send_control_command(0x00)
            elif self.connection_mode == "OneNet MQTT":
                 if self.comm_worker and hasattr(self.comm_worker, 'send_data'):
                    payload = json.dumps({"nav_mode": 0, "direction": 0}) # 同时退出导航并停止
                    self.comm_worker.send_data(payload)
                    self.log(f"通过MQTT发送指令: 退出导航模式并停止")
                 else:
                    self.log("MQTT命令发送失败: 未连接。", level='ERROR')
            elif self.connection_mode == "自定义TCP服务器":
                if self.comm_worker and hasattr(self.comm_worker, 'send_control_command'):
                    self.comm_worker.send_control_command("set_navigation", {"mode": 0})
                    self.comm_worker.send_control_command("move", {"direction": "stop"})
                    self.log("通过TCP发送指令: 退出导航模式并停止")
                else:
                    self.log("TCP命令发送失败: 未连接。", level='ERROR')
            elif self.connection_mode == "OneNet API (指令)":
                payload = {"nav_mode": 0, "direction": 0}
                self.send_command_via_api(json.dumps(payload))

            self.nav_button.setText("开始导航")
            self.nav_status_label.setText("状态: 未激活")
            self.log("自动导航已停止。")

    def navigate_to_destination(self):
        if self.destination is None:
            self.log("导航错误: 目标点丢失。", level='ERROR')
            self.nav_button.click()
            return
        
        current_lat = self.latest_data['lat']
        current_lon = self.latest_data['lon']
        
        if current_lat == 0.0 and current_lon == 0.0:
            self.log("导航等待: 无效的当前GPS坐标。", level='INFO')
            return

        dest_lat, dest_lon = self.destination
        
        # 简易距离检查 (单位: 度)
        lat_diff = current_lat - dest_lat
        lon_diff = current_lon - dest_lon
        # 近似距离，在小范围内可用
        distance_m = math.sqrt(lat_diff**2 + lon_diff**2) * 111320 

        if distance_m < 3: # 到达阈值: 3米
            self.log(f"已到达目标点! 距离: {distance_m:.2f}米", level='SUCCESS')
            self.nav_button.click() # 自动停止导航
            return

        target_bearing = self.calculate_bearing(current_lat, current_lon, dest_lat, dest_lon)
        
        self.log(f"导航更新: 距离目标 {distance_m:.1f}米, 计算航向: {target_bearing:.2f}°")

        # 使用现有的 '设置目标偏航角' 指令 (0x82) / 或其MQTT等效指令
        # 为了复用，我们直接调用已经改造好的 send_target_yaw_command
        self.yaw_spinbox.setValue(target_bearing) # 更新UI，这样用户能看到当前的导航目标
        self.send_target_yaw_command() # 发送指令

    def calculate_bearing(self, lat1, lon1, lat2, lon2):
        """计算从点1到点2的方位角 (0-360度)"""
        lat1_rad, lon1_rad = math.radians(lat1), math.radians(lon1)
        lat2_rad, lon2_rad = math.radians(lat2), math.radians(lon2)
        
        dLon = lon2_rad - lon1_rad
        
        y = math.sin(dLon) * math.cos(lat2_rad)
        x = math.cos(lat1_rad) * math.sin(lat2_rad) - math.sin(lat1_rad) * math.cos(lat2_rad) * math.cos(dLon)
        
        bearing = math.atan2(y, x)
        bearing = math.degrees(bearing)
        return (bearing + 360) % 360

    def closeEvent(self, event):
        if self.is_logging:
            self.toggle_logging(False) # Ensure log file is closed
        if self.nav_timer.isActive():
            self.toggle_navigation(False) # Stop navigation
        # 在退出前执行断开连接的逻辑
        if self.connect_button.isChecked():
            self.disconnect()
        event.accept()

    def keyPressEvent(self, event):
        """Handle keyboard presses for vehicle control."""
        if not self.connect_button.isChecked() or event.isAutoRepeat():
            return

        key = event.key()

        if key == Qt.Key.Key_W:
            self.send_control_command(0x01) # Forward
            self.log("键盘: 前进")
        elif key == Qt.Key.Key_S:
            self.send_control_command(0x02) # Backward
            self.log("键盘: 后退")
        elif key == Qt.Key.Key_A:
            self.send_control_command(0x03) # Left
            self.log("键盘: 左转")
        elif key == Qt.Key.Key_D:
            self.send_control_command(0x04) # Right
            self.log("键盘: 右转")
        else:
            super().keyPressEvent(event) # Pass other keys to parent

    def keyReleaseEvent(self, event):
        """Handle keyboard releases to stop the vehicle."""
        if not self.connect_button.isChecked() or event.isAutoRepeat():
            return

        key = event.key()

        # Stop the car if any of the movement keys are released
        if key in [Qt.Key.Key_W, Qt.Key.Key_A, Qt.Key.Key_S, Qt.Key.Key_D]:
            self.send_control_command(0x00) # Stop
            self.log("键盘: 停止")
        else:
            super().keyReleaseEvent(event) # Pass other keys to parent

    @pyqtSlot(dict)
    def handle_pulsar_data(self, data):
        """处理从 Pulsar Java 进程接收到的已解析的 JSON 数据。"""
        self.log(f"PULSAR_DATA: {data}", level='RX')
        
        processed_data = {}

        # 数据结构是嵌套的: {"msgType":"thingProperty","subData":{"params":{...}}}
        if data.get('msgType') == 'thingProperty':
            params = data.get('subData', {}).get('params', {})
            
            # 从 Pulsar JSON 到内部键的映射
            # Pulsar 的键似乎是小写的
            key_map = {
                'longitude': 'lon', 
                'latitude': 'lat',
                'gps_speed': 'gps_speed', 
                'satellites': 'satellites',
                'pitch': 'pitch', 
                'roll': 'roll', 
                'yaw': 'yaw',
                'heading': 'heading',
                'motor_l': 'motorL', 
                'motor_r': 'motorR',
                'battery': 'battery',
                'mpu_status': 'mpu', 
                'hmc_status': 'hmc', 
                'gps_status': 'gps',
                'temp': 'temp' # 即使不在UI上，也记录下来
            }

            for pulsar_key, value_dict in params.items():
                if isinstance(value_dict, dict) and 'value' in value_dict:
                    # 将 Pulsar 的小写键映射到我们的内部键
                    if pulsar_key.lower() in key_map:
                        internal_key = key_map[pulsar_key.lower()]
                        processed_data[internal_key] = value_dict['value']

        if processed_data:
            self.update_ui_with_data(processed_data)
        else:
            msg_type = data.get('msgType', 'N/A')
            self.log(f"Pulsar 消息 (类型: {msg_type}) 未被解析或不包含有效的'thingProperty'数据。", level='INFO')

    def handle_mqtt_data(self, data):
        """处理从MQTT获取的数据"""
        self.log(f"MQTT数据: {data}", level='RX')
        
        # MQTT数据结构可能有所不同，这里需要根据实际情况调整
        processed_data = {}
        
        # 处理各种可能的数据格式
        if isinstance(data, dict):
            # 直接处理params字典
            for key, value_obj in data.items():
                if isinstance(value_obj, dict) and 'value' in value_obj:
                    value = value_obj['value']
                    
                    # 映射键名到内部使用的键
                    internal_key = key
                    if key == 'gps_lat':
                        internal_key = 'lat'
                    elif key == 'gps_lon':
                        internal_key = 'lon'
                    elif key == 'motor_l':
                        internal_key = 'motorL'
                    elif key == 'motor_r':
                        internal_key = 'motorR'
                    
                    processed_data[internal_key] = value
        
        if processed_data:
            self.update_ui_with_data(processed_data)

    def send_universal_command(self):
        """通过通用发送框发送指令（适用于MQTT和API模式）"""
        command_str = self.universal_send_input.text().strip()
        if not command_str:
            return
            
        # 验证是否已连接
        if not self.comm_worker:
            self.log("发送失败: 未连接到任何服务。", level='ERROR')
            return
            
        try:
            if self.connection_mode == "OneNet MQTT":
                if hasattr(self.comm_worker, 'send_data'):
                    self.comm_worker.send_data(command_str)
                    self.log(f"通过MQTT发送: {command_str}", level='TX')
                else:
                    self.log("发送失败: MQTT客户端未准备就绪。", level='ERROR')
                    
            elif self.connection_mode == "OneNet API (指令)":
                self.send_command_via_api(command_str)
                
            elif self.connection_mode == "自定义TCP服务器":
                if hasattr(self.comm_worker, 'send_message'):
                    # 尝试解析为JSON对象
                    try:
                        msg_dict = json.loads(command_str)
                        self.comm_worker.send_message(msg_dict)
                    except json.JSONDecodeError:
                        # 非JSON文本，作为普通字符串命令发送
                        self.comm_worker.send_message({"cmd": "text_command", "text": command_str})
                    self.log(f"通过TCP发送: {command_str}", level='TX')
                else:
                    self.log("发送失败: TCP客户端未准备就绪。", level='ERROR')
                    
            else:
                self.log(f"无法发送: 当前连接模式 [{self.connection_mode}] 不支持云端指令。", level='ERROR')
                
        except Exception as e:
            self.log(f"发送命令时出错: {e}", level='ERROR')

    def handle_onenet_push_data(self, data):
        """处理来自 OneNet HTTP Push 的数据"""
        self.log(f"HTTP_PUSH_DATA: {data}", level='RX')

        processed_data = {}
        # OneNet 推送的数据体中可能包含多种类型，这里主要处理设备数据点
        # 根据实际推送的数据格式可能需要调整
        if data.get('type') == 1 and 'payload' in data: # type=1 通常是设备数据点
             payload = data['payload']
             # 这里的payload可能就是我们需要的键值对
             # 需要根据实际推送情况调整
             key_map = {
                'lon': 'lon', 'lat': 'lat', 'gps_speed': 'gps_speed', 'satellites': 'satellites',
                'pitch': 'pitch', 'roll': 'roll', 'yaw': 'yaw', 'heading': 'heading',
                'motor_l': 'motorL', 'motor_r': 'motorR', 'battery': 'battery',
                'mpu_status': 'mpu', 'hmc_status': 'hmc', 'gps_status': 'gps'
             }
             for key, value in payload.items():
                 if key in key_map:
                     internal_key = key_map[key]
                     processed_data[internal_key] = value

        # 如果数据直接就是键值对
        elif isinstance(data, dict):
            key_map = {
                'longitude': 'lon', 'latitude': 'lat',
                # ... 其他可能的键映射
            }
            for key, value in data.items():
                if key in key_map:
                    processed_data[key_map[key]] = value
                else:
                    processed_data[key] = value # 直接使用

        if processed_data:
            self.update_ui_with_data(processed_data)
        else:
            self.log(f"未能从HTTP Push数据中解析出有效内容: {data}", level='INFO')

    def handle_relay_data(self, data):
        """处理来自云服务中继 (MQTT) 的数据"""
        self.log(f"RELAY_DATA: {data}", level='RX')
        
        processed_data = {}
        if isinstance(data, dict) and 'data' in data and 'params' in data.get('data', {}):
            params = data['data']['params']
            
            # Key mapping for consistency, though many keys are the same
            key_map = {
                'longitude': 'lon', 'latitude': 'lat', 'gps_speed': 'gps_speed', 'satellites': 'satellites',
                'pitch': 'pitch', 'roll': 'roll', 'yaw': 'yaw', 'heading': 'heading',
                'motor_l': 'motorL', 'motor_r': 'motorR', 'battery': 'battery',
                'mpu_status': 'mpu', 'hmc_status': 'hmc', 'gps_status': 'gps', 'temp': 'temp'
            }
            
            for received_key, value_dict in params.items():
                if isinstance(value_dict, dict) and 'value' in value_dict:
                    # Use internal key if mapping exists, otherwise use received key
                    internal_key = key_map.get(received_key, received_key)
                    processed_data[internal_key] = value_dict['value']

        if processed_data:
            self.update_ui_with_data(processed_data)
        else:
            self.log(f"未能从中继数据中解析出有效内容: {data}", level='INFO')

    def connect_onenet_api_cmd(self):
        user_id = self.api_cmd_user_id_input.text()
        access_key = self.api_cmd_access_key_input.text()
        product_id = self.api_cmd_product_id_input.text()
        device_name = self.api_cmd_device_name_input.text()

        if not all([user_id, access_key, product_id, device_name]):
            self.log("API指令模式配置不完整，请输入所有字段。", level='ERROR')
            self.connect_button.setChecked(False)
            return
        
        self.api_cmd_config['user_id'] = user_id
        self.api_cmd_config['access_key'] = access_key
        self.api_cmd_config['product_id'] = product_id
        self.api_cmd_config['device_name'] = device_name
        
        self.set_connection_widgets_enabled(False)
        self.refresh_api_button.setEnabled(True)
        self.api_poll_timer.start()
        self.log(f"OneNet API指令模式已配置。User ID: {user_id}, 设备: {device_name}", level='SUCCESS')
        self.log("API轮询已启动（每1秒一次）。")
    
    def generate_onenet_api_token(self, user_id, access_key):
        """根据OneNet文档生成动态的Authorization Token。"""
        version = '2022-05-01'
        # 使用用户级鉴权
        res = f'userid/{user_id}'
        # Token有效期为1小时
        et = str(int(time.time()) + 3600)
        # 签名方法，sha1
        method = 'sha1'
        
        try:
            # 对access_key进行decode
            key = base64.b64decode(access_key)
            # 构造待签名的字符串
            org = f"{et}\n{method}\n{res}\n{version}"
            
            # 计算签名
            sign_b = hmac.new(key=key, msg=org.encode('utf-8'), digestmod=method)
            sign = base64.b64encode(sign_b.digest()).decode()
            
            # 对value部分进行url编码
            sign = quote(sign, safe='')
            res_encoded = quote(res, safe='')
            
            # 拼接最终的token字符串
            token = f'version={version}&res={res_encoded}&et={et}&method={method}&sign={sign}'
            return token
        except Exception as e:
            self.log(f"生成Token失败: {e}", level="ERROR")
            return None

    def send_command_via_api(self, command_payload_str):
        config = self.api_cmd_config
        if not all(config.values()):
            self.log("API配置不完整，无法发送命令。", level='ERROR')
            return
            
        token = self.generate_onenet_api_token(config['user_id'], config['access_key'])
        if not token:
            return

        url = f"https://iot-api.heclouds.com/datapoint/synccmds?product_id={config['product_id']}&device_name={config['device_name']}&timeout=10"
        headers = {
            'Authorization': token,
            'Content-Type': 'application/json' # 明确指定Content-Type
        }
        
        try:
            self.log(f"API_TX: URL={url}, Body={command_payload_str}", level='TX')
            # 根据文档，body是命令字符串，可以是一个JSON字符串
            response = requests.post(url, headers=headers, data=command_payload_str.encode('utf-8'), timeout=15)
            
            self.log(f"API_RX: Status={response.status_code}, Body={response.text}", level='RX')

            if response.status_code == 200:
                response_data = response.json()
                if response_data.get("code") == 0:
                    self.log(f"API 命令发送成功。", level='SUCCESS')
                    if 'data' in response_data and 'cmd_resp' in response_data['data']:
                        try:
                            decoded_resp = base64.b64decode(response_data['data']['cmd_resp']).decode('utf-8')
                            self.log(f"设备响应: {decoded_resp}", level='FIRMWARE')
                        except Exception as e:
                            self.log(f"解析设备响应失败: {e}", level='ERROR')
                else:
                    err_code = response_data.get("code")
                    err_msg = response_data.get('msg', '未知错误')
                    # 特别处理鉴权错误
                    if err_code == 10403:
                         err_msg += " (鉴权失败，请检查User ID和Access Key是否正确，以及该Key是否有权限操作此设备)"
                    self.log(f"API 命令返回错误: {err_msg} (code: {err_code})", level='ERROR')
            else:
                self.log(f"API HTTP请求失败，状态码: {response.status_code}", level='ERROR')

        except requests.exceptions.RequestException as e:
            self.log(f"API 网络请求错误: {e}", level='ERROR')
        except Exception as e:
            self.log(f"发送API命令时发生未知错误: {e}", level='ERROR')

    def poll_device_properties_via_api(self):
        config = self.api_cmd_config
        if not all(config.values()):
            # Don't log this every second, it's spammy
            # self.log("API配置不完整，无法查询状态。", level='ERROR')
            return

        token = self.generate_onenet_api_token(config['user_id'], config['access_key'])
        if not token:
            return

        url = f"https://iot-api.heclouds.com/thingmodel/query-device-property?product_id={config['product_id']}&device_name={config['device_name']}"
        headers = {'Authorization': token}

        try:
            # self.log("正在通过API查询设备属性...", level='INFO')
            response = requests.get(url, headers=headers, timeout=10)

            if response.status_code == 200:
                response_data = response.json()
                if response_data.get("code") == 0 and 'data' in response_data:
                    # self.log("设备属性查询成功。", level='SUCCESS')
                    self.handle_api_property_data(response_data['data'])
                else:
                    err_code = response_data.get("code")
                    err_msg = response_data.get('msg', '未知错误')
                    self.log(f"API属性查询返回错误: {err_msg} (code: {err_code})", level='ERROR')
            else:
                self.log(f"API属性查询HTTP请求失败，状态码: {response.status_code}", level='ERROR')

        except requests.exceptions.RequestException as e:
            self.log(f"API属性查询网络请求错误: {e}", level='ERROR')

    def handle_api_property_data(self, property_list):
        processed_data = {}
        # OneNet物模型标识符到我们内部键的映射
        key_map = {
            'longitude': 'lon', 'latitude': 'lat', 'gps_speed': 'gps_speed', 'satellites': 'satellites',
            'pitch': 'pitch', 'roll': 'roll', 'yaw': 'yaw', 'heading': 'heading',
            'motor_l': 'motorL', 'motor_r': 'motorR', 'battery': 'battery',
            'mpu_status': 'mpu', 'hmc_status': 'hmc', 'gps_status': 'gps'
        }
        
        for prop in property_list:
            identifier = prop.get('identifier')
            if identifier in key_map:
                internal_key = key_map[identifier]
                try:
                    # 尝试将value转换为数值类型
                    value = float(prop.get('value'))
                    # 如果是整数，则转换为int
                    if value.is_integer():
                        value = int(value)
                    processed_data[internal_key] = value
                except (ValueError, TypeError):
                    # 如果转换失败，保留原始值
                    processed_data[internal_key] = prop.get('value')
        
        if processed_data:
            self.log(f"API_PROPERTY_RX: {processed_data}", level='RX')
            self.update_ui_with_data(processed_data)
        else:
            self.log("未从API轮询中获取到有效属性数据。", level='INFO')

    @pyqtSlot(dict)
    def handle_tcp_data(self, data):
        """
        处理从TCP服务器接收的JSON数据
        
        根据新协议处理 'control_ack' 和 'error' 响应
        
        Args:
            data: 解析后的JSON数据字典
        """
        self.log(f"TCP_DATA: {data}", level='RX')
        
        cmd = data.get("cmd")
        request_id = data.get("request_id", "N/A")

        if cmd == "control_ack":
            status = data.get("status")
            device_response = data.get("device_response", {})
            log_level = 'SUCCESS' if status == 'success' else 'INFO'
            self.log(f"指令响应 (ID: {request_id}): 状态={status}, 设备响应={device_response}", level=log_level)

        elif cmd == "error":
            code = data.get("code")
            message = data.get("message", "未知错误")
            self.log(f"服务器错误 (ID: {request_id}): {message} (code: {code})", level='ERROR')
        
        # 保留对旧的 data_update 的兼容性，以防服务器也发送此类型数据
        elif cmd == "data_update":
             processed_data = data.get("data", {})
             if processed_data:
                 self.update_ui_with_data(processed_data)
        
        else:
             self.log(f"收到未知TCP指令: {cmd}, 内容: {data}", level='INFO')


# ==============================================================================
# 自定义 TCP 通信工作线程 (修改后)
# ==============================================================================
class CustomTcpWorker(QObject):
    """
    自定义TCP客户端工作线程，用于连接远程TCP服务器
    使用换行符分隔的JSON格式进行通信
    """
    data_received = pyqtSignal(dict)
    log_message = pyqtSignal(str)
    
    def __init__(self, server_address, server_port, device_id):
        """
        初始化TCP客户端
        
        Args:
            server_address: 服务器地址
            server_port: 服务器端口
            device_id: 设备唯一标识符
        """
        super().__init__()
        self.server_address = server_address
        self.server_port = int(server_port)
        self.device_id = device_id
        self.socket = None
        self.running = False
        
    def run(self):
        # TCP通信主循环，接收以换行符分隔的JSON数据
        try:
            self.running = True
            self.log_message.emit(f"正在连接到TCP服务器 {self.server_address}:{self.server_port}...")
            
            self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.socket.settimeout(10)  # 连接超时
            self.socket.connect((self.server_address, self.server_port))
            self.socket.settimeout(1)  # 将超时调整为较小的值用于接收循环
            self.log_message.emit("成功连接到TCP服务器")
            
            # 接收循环
            buffer = b""
            while self.running:
                try:
                    # 从套接字接收数据
                    chunk = self.socket.recv(4096)
                    if not chunk:
                        if self.running:
                           self.log_message.emit("与服务器的连接已断开")
                        break
                        
                    buffer += chunk
                    
                    # 处理缓冲区中所有完整的消息
                    while b'\n' in buffer:
                        message_bytes, buffer = buffer.split(b'\n', 1)
                        message_str = message_bytes.decode('utf-8').strip()
                        
                        if message_str:
                            try:
                                msg = json.loads(message_str)
                                # self.log_message.emit(f"收到服务器消息: {msg}")
                                self.data_received.emit(msg)
                            except json.JSONDecodeError:
                                self.log_message.emit(f"收到非JSON数据: {message_str}")
                
                except socket.timeout:
                    continue # 非阻塞模式，超时是正常的
                except Exception as e:
                    if self.running:
                        self.log_message.emit(f"接收数据时发生错误: {e}")
                    break
                    
        except Exception as e:
            if self.running:
                self.log_message.emit(f"连接或通信时发生错误: {e}")
        
        finally:
            if self.socket:
                try:
                    self.socket.close()
                except:
                    pass
            self.socket = None
            if self.running:
                self.log_message.emit("TCP连接已关闭")
            
    def send_message(self, message_dict):
        """
        发送格式化的JSON消息到服务器
        
        将JSON消息序列化，并附加一个换行符
        
        Args:
            message_dict: 要发送的消息字典
            
        Returns:
            bool: 发送成功返回True，否则返回False
        """
        if not self.running or not self.socket:
            self.log_message.emit("发送失败: TCP未连接")
            return False
            
        try:
            # 序列化为JSON字符串并添加换行符
            message_json = json.dumps(message_dict)
            message_to_send = (message_json + '\n').encode('utf-8')
            
            # 发送消息
            self.socket.sendall(message_to_send)
            self.log_message.emit(f"发送消息: {message_json}")
            return True
            
        except Exception as e:
            self.log_message.emit(f"发送消息时发生错误: {e}")
            self.stop() # 发送失败时，很可能连接已断开，主动关闭
            return False
    
    def send_control_command(self, action, params):
        """
        发送控制命令到服务器
        
        构造符合新协议的控制请求消息并发送
        
        Args:
            action: 控制动作类型，如"move", "set_speed"等
            params: 控制参数字典
            
        Returns:
            bool: 发送成功返回True，否则返回False
        """
        request_id = f"req_{int(time.time())}_{os.urandom(3).hex()}"

        # 组合 data 字段，包含 action 和其他参数
        data_payload = {"action": action}
        data_payload.update(params)

        control_msg = {
            "cmd": "control_request",
            "request_id": request_id,
            "device_id": self.device_id,
            "data": data_payload
        }
        return self.send_message(control_msg)
    
    def stop(self):
        """停止工作线程"""
        if self.running:
            self.running = False
            if self.socket:
                try:
                    # 新协议未定义注销，直接关闭
                    self.socket.shutdown(socket.SHUT_RDWR)
                except OSError:
                    pass # 忽略套接字已关闭的错误
                finally:
                    try:
                        self.socket.close()
                    except:
                        pass
            self.socket = None


# ==============================================================================
# 主窗口
# ==============================================================================


if __name__ == '__main__':
    # 确保在高DPI屏幕上UI能正常缩放
    if hasattr(Qt.ApplicationAttribute, 'AA_EnableHighDpiScaling'):
        QApplication.setAttribute(Qt.ApplicationAttribute.AA_EnableHighDpiScaling, True)
    if hasattr(Qt.ApplicationAttribute, 'AA_UseHighDpiPixmaps'):
        QApplication.setAttribute(Qt.ApplicationAttribute.AA_UseHighDpiPixmaps, True)
        
    app = QApplication(sys.argv)
    window = MainWindow()
    window.show()
    sys.exit(app.exec()) 